From 088e59a68bcdc85ab385fae34e0176e365d5c202 Mon Sep 17 00:00:00 2001 From: Marco Boneberger Date: Fri, 9 Sep 2022 09:58:04 +0200 Subject: [PATCH 01/63] wip implement read --- examples/echo_robot_state.rs | 11 +- examples/generate_consecutive_motions.rs | 4 +- examples/generate_elbow_motion.rs | 4 +- examples/generate_joint_position_motion.rs | 4 +- examples/generate_joint_velocity_motion.rs | 4 +- examples/mirror_robot.rs | 4 +- src/lib.rs | 18 +-- src/model.rs | 14 +-- src/network.rs | 6 +- src/robot.rs | 132 ++++++++++++--------- src/robot/control_loop.rs | 12 +- src/robot/control_types.rs | 14 +-- src/robot/logger.rs | 8 +- src/robot/robot_control.rs | 6 +- src/robot/robot_impl.rs | 50 +++++--- src/robot/robot_state.rs | 10 +- src/robot/types.rs | 6 +- src/utils.rs | 8 +- 18 files changed, 182 insertions(+), 133 deletions(-) diff --git a/examples/echo_robot_state.rs b/examples/echo_robot_state.rs index d1f552c..3e54c13 100644 --- a/examples/echo_robot_state.rs +++ b/examples/echo_robot_state.rs @@ -2,9 +2,10 @@ // Licensed under the EUPL-1.2-or-later use clap::Parser; -use franka::FrankaResult; -use franka::Robot; -use franka::RobotState; +use franka::{FrankaResult, Panda, PandaState}; +use franka::robot::Robot; +// use franka::Robot; +// use franka::RobotState; /// An example showing how to continuously read the robot state. #[derive(Parser, Debug)] @@ -16,9 +17,9 @@ struct CommandLineArguments { fn main() -> FrankaResult<()> { let address = CommandLineArguments::parse(); - let mut robot = Robot::new(address.franka_ip.as_str(), None, None)?; + let mut robot = Panda::new(address.franka_ip.as_str(), None, None)?; let mut count = 0; - robot.read(|robot_state: &RobotState| { + robot.read(|robot_state: &PandaState| { // Printing to standard output adds a delay. This is acceptable for a read loop such as this, but // should not be done in a control loop. println!("{:?}", robot_state); diff --git a/examples/generate_consecutive_motions.rs b/examples/generate_consecutive_motions.rs index ef1bb24..64022d9 100644 --- a/examples/generate_consecutive_motions.rs +++ b/examples/generate_consecutive_motions.rs @@ -3,7 +3,7 @@ use clap::Parser; use franka::exception::FrankaException; -use franka::{FrankaResult, JointVelocities, MotionFinished, Robot, RobotState}; +use franka::{FrankaResult, JointVelocities, MotionFinished, Robot, PandaState}; use std::f64::consts::PI; use std::time::Duration; @@ -45,7 +45,7 @@ fn main() -> FrankaResult<()> { let mut time = 0.; let omega_max = 0.2; let time_max = 4.0; - let callback = move |_state: &RobotState, time_step: &Duration| -> JointVelocities { + let callback = move |_state: &PandaState, time_step: &Duration| -> JointVelocities { time += time_step.as_secs_f64(); let cycle = f64::floor(f64::powf( diff --git a/examples/generate_elbow_motion.rs b/examples/generate_elbow_motion.rs index adb5232..8f7f84e 100644 --- a/examples/generate_elbow_motion.rs +++ b/examples/generate_elbow_motion.rs @@ -2,7 +2,7 @@ // Licensed under the EUPL-1.2-or-later use clap::Parser; -use franka::{CartesianPose, FrankaResult, MotionFinished, Robot, RobotState}; +use franka::{CartesianPose, FrankaResult, MotionFinished, Robot, PandaState}; use std::f64::consts::PI; use std::time::Duration; @@ -42,7 +42,7 @@ fn main() -> FrankaResult<()> { let mut initial_pose = [0.; 16]; let mut initial_elbow = [0.; 2]; let mut time = 0.; - let callback = |state: &RobotState, time_step: &Duration| -> CartesianPose { + let callback = |state: &PandaState, time_step: &Duration| -> CartesianPose { time += time_step.as_secs_f64(); if time == 0. { initial_pose = state.O_T_EE_c; diff --git a/examples/generate_joint_position_motion.rs b/examples/generate_joint_position_motion.rs index f786529..6ab8d2a 100644 --- a/examples/generate_joint_position_motion.rs +++ b/examples/generate_joint_position_motion.rs @@ -2,7 +2,7 @@ // Licensed under the EUPL-1.2-or-later use clap::Parser; -use franka::{FrankaResult, JointPositions, MotionFinished, Robot, RobotState}; +use franka::{FrankaResult, JointPositions, MotionFinished, Robot, PandaState}; use std::f64::consts::PI; use std::time::Duration; @@ -42,7 +42,7 @@ fn main() -> FrankaResult<()> { 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 time = 0.; - let callback = |state: &RobotState, time_step: &Duration| -> JointPositions { + let callback = |state: &PandaState, time_step: &Duration| -> JointPositions { time += time_step.as_secs_f64(); if time == 0. { initial_position.q = state.q_d; diff --git a/examples/generate_joint_velocity_motion.rs b/examples/generate_joint_velocity_motion.rs index 013c638..ba81a40 100644 --- a/examples/generate_joint_velocity_motion.rs +++ b/examples/generate_joint_velocity_motion.rs @@ -4,7 +4,7 @@ use clap::Parser; use franka::FrankaResult; use franka::Robot; -use franka::RobotState; +use franka::PandaState; use franka::{JointVelocities, MotionFinished}; use std::f64::consts::PI; use std::time::Duration; @@ -46,7 +46,7 @@ fn main() -> FrankaResult<()> { let mut time = 0.; let omega_max = 1.0; let time_max = 1.0; - let callback = move |_state: &RobotState, time_step: &Duration| -> JointVelocities { + let callback = move |_state: &PandaState, time_step: &Duration| -> JointVelocities { time += time_step.as_secs_f64(); let cycle = f64::floor(f64::powf( diff --git a/examples/mirror_robot.rs b/examples/mirror_robot.rs index 4caf3bb..9210889 100644 --- a/examples/mirror_robot.rs +++ b/examples/mirror_robot.rs @@ -8,7 +8,7 @@ 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 franka::PandaState; use nalgebra::{Matrix3, Matrix6, Matrix6x1, Quaternion, UnitQuaternion, Vector3, U1, U3}; use std::sync::mpsc::channel; use std::time::Duration; @@ -99,7 +99,7 @@ fn main() -> FrankaResult<()> { }); robot_mirror.control_torques( - |state: &RobotState, _step: &Duration| -> Torques { + |state: &PandaState, _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); diff --git a/src/lib.rs b/src/lib.rs index 9ce3423..971a317 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, PandaState, Panda, FrankaResult}; //! fn main() -> FrankaResult<()> { -//! let mut robot = Robot::new("robotik-bs.de", None, None)?; +//! let mut robot = Panda::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], @@ -35,7 +35,7 @@ //! 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 { +//! let callback = |state: &PandaState, time_step: &Duration| -> JointPositions { //! time += time_step.as_secs_f64(); //! if time == 0. { //! initial_position.q = state.q_d; @@ -57,10 +57,10 @@ //! 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, PandaState, Panda, FrankaResult}; //! # fn main() -> FrankaResult<()> { //! let mut robot = Robot::new("robotik-bs.de", None, None)?; //! # Ok(()) @@ -77,10 +77,10 @@ //! ``` //! 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, PandaState, Robot, FrankaResult}; //! # fn main() -> FrankaResult<()> { //! # let mut robot = Robot::new("robotik-bs.de", None, None)?; //! let q_goal = [0., -PI / 4., 0., -3. * PI / 4., 0., PI / 2., PI / 4.]; @@ -179,6 +179,6 @@ pub use model::Model; 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::Robot; +pub use robot::robot_state::PandaState; +pub use robot::Panda; pub use utils::*; diff --git a/src/model.rs b/src/model.rs index 191fc13..6a266cb 100644 --- a/src/model.rs +++ b/src/model.rs @@ -5,7 +5,7 @@ use nalgebra::Matrix4; use crate::model::model_library::ModelLibrary; -use crate::robot::robot_state::RobotState; +use crate::robot::robot_state::PandaState; use crate::FrankaResult; use std::fmt; use std::path::Path; @@ -155,7 +155,7 @@ impl Model { /// * `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] { + pub fn pose_from_state(&self, frame: &Frame, robot_state: &PandaState) -> [f64; 16] { self.pose( frame, &robot_state.q, @@ -213,7 +213,7 @@ impl Model { /// * `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] { + pub fn body_jacobian_from_state(&self, frame: &Frame, robot_state: &PandaState) -> [f64; 42] { self.body_jacobian( frame, &robot_state.q, @@ -270,7 +270,7 @@ impl Model { /// * `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] { + pub fn zero_jacobian_from_state(&self, frame: &Frame, robot_state: &PandaState) -> [f64; 42] { self.zero_jacobian( frame, &robot_state.q, @@ -304,7 +304,7 @@ impl Model { /// * `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] { + pub fn mass_from_state(&self, robot_state: &PandaState) -> [f64; 49] { self.mass( &robot_state.q, &robot_state.I_total, @@ -343,7 +343,7 @@ impl Model { /// * `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] { + pub fn coriolis_from_state(&self, robot_state: &PandaState) -> [f64; 7] { self.coriolis( &robot_state.q, &robot_state.dq, @@ -383,7 +383,7 @@ impl Model { /// Gravity vector. pub fn gravity_from_state<'a, Grav: Into>>( &self, - robot_state: &RobotState, + robot_state: &PandaState, gravity_earth: Grav, ) -> [f64; 7] { self.gravity( diff --git a/src/network.rs b/src/network.rs index ec999ea..fd26f95 100644 --- a/src/network.rs +++ b/src/network.rs @@ -400,12 +400,12 @@ fn deserialize(encoded: &[u8]) -> T { #[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..01711af 100644 --- a/src/robot.rs +++ b/src/robot.rs @@ -18,7 +18,7 @@ use crate::robot::control_types::{ use crate::robot::low_pass_filter::{DEFAULT_CUTOFF_FREQUENCY, MAX_CUTOFF_FREQUENCY}; use crate::robot::motion_generator_traits::MotionGeneratorTrait; use crate::robot::robot_control::RobotControl; -use crate::robot::robot_impl::RobotImpl; +use crate::robot::robot_impl::{RobotImpl, RobotImplementation}; use crate::robot::service_types::{ AutomaticErrorRecoveryRequestWithHeader, AutomaticErrorRecoveryResponse, AutomaticErrorRecoveryStatus, GetCartesianLimitRequest, GetCartesianLimitRequestWithHeader, @@ -34,7 +34,7 @@ use crate::robot::service_types::{ }; use crate::robot::virtual_wall_cuboid::VirtualWallCuboid; use crate::utils::MotionGenerator; -use robot_state::RobotState; +use robot_state::PandaState; mod control_loop; mod control_tools; @@ -52,6 +52,24 @@ pub(crate) mod service_types; pub(crate) mod types; pub mod virtual_wall_cuboid; +pub trait Robot { + type State:Debug; + type Rob:RobotImplementation; + fn get_rob(&mut self) -> &mut Self::Rob; + fn read bool>(&mut self, + mut read_callback: F, + ) -> FrankaResult<()> { + loop { + + let state = self.get_rob().update2(None, None)?; + if !read_callback(&state) { + break; + } + } + Ok(()) + } +} + /// Maintains a network connection to the robot, provides the current robot state, gives access to /// the model library and allows to control the robot. /// # Nominal end effector frame NE @@ -87,12 +105,12 @@ pub mod virtual_wall_cuboid; /// The following incomplete example shows the general structure of a callback function: /// /// ```no_run -/// use franka::robot::robot_state::RobotState; +/// use franka::robot::robot_state::PandaState; /// 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])} /// let mut time = 0.; -/// let callback = |state: &RobotState, time_step: &Duration| -> JointPositions { +/// let callback = |state: &PandaState, time_step: &Duration| -> JointPositions { /// time += time_step.as_secs_f64(); /// let out: JointPositions = your_function_which_generates_joint_positions(time); /// if time >= 5.0 { @@ -105,11 +123,19 @@ 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 { +pub struct Panda { robimpl: RobotImpl, } -impl Robot { +impl Robot for Panda { + type State = PandaState; + type Rob = RobotImpl; + fn get_rob(&mut self) ->&mut Self::Rob { + &mut self.robimpl + } +} + +impl Panda { /// Establishes a connection with the robot. /// /// # Arguments @@ -120,12 +146,12 @@ impl Robot { /// The log is provided when a [`ControlException`](`crate::exception::FrankaException::ControlException`) is thrown. /// # Example /// ```no_run - /// use franka::{FrankaResult, RealtimeConfig, Robot}; + /// 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 = Robot::new("robotik-bs.de", None, None)?; + /// 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 = Robot::new("robotik-bs.de", RealtimeConfig::Ignore, 1)?; + /// let mut robot = Panda::new("robotik-bs.de", RealtimeConfig::Ignore, 1)?; /// Ok(()) /// } /// ``` @@ -136,7 +162,7 @@ impl Robot { franka_address: &str, realtime_config: RtConfig, log_size: LogSize, - ) -> FrankaResult { + ) -> FrankaResult { let realtime_config = realtime_config.into().unwrap_or(RealtimeConfig::Enforce); let log_size = log_size.into().unwrap_or(50); let network = Network::new( @@ -147,7 +173,7 @@ impl Robot { .map_err(|_| FrankaException::NetworkException { message: "Connection could not be established".to_string(), })?; - Ok(Robot { + Ok(Panda { robimpl: RobotImpl::new(network, log_size, realtime_config)?, }) } @@ -157,11 +183,11 @@ impl Robot { /// /// This minimal example will print the robot state 100 times: /// ```no_run - /// use franka::{Robot, RobotState, FrankaResult}; + /// use franka::{Panda, PandaState, FrankaResult}; /// fn main() -> FrankaResult<()> { - /// let mut robot = Robot::new("robotik-bs.de",None,None)?; + /// let mut robot = Panda::new("robotik-bs.de",None,None)?; /// let mut count = 0; - /// robot.read(| robot_state:&RobotState | -> bool { + /// robot.read(| robot_state:&PandaState | -> bool { /// println!("{:?}", robot_state); /// count += 1; /// count <= 100 @@ -173,18 +199,18 @@ impl Robot { /// as it wants to receive further robot states. /// # Error /// * [`NetworkException`](`crate::exception::FrankaException::NetworkException`) if the connection is lost, e.g. after a timeout. - pub fn read bool>( - &mut self, - mut read_callback: F, - ) -> FrankaResult<()> { - loop { - let state = self.robimpl.update(None, None)?; - if !read_callback(&state) { - break; - } - } - Ok(()) - } + // pub fn read bool>( + // &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. /// /// # Return @@ -193,7 +219,7 @@ impl Robot { /// * [`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 { + pub fn read_once(&mut self) -> FrankaResult { self.robimpl.read_once() } @@ -611,7 +637,7 @@ impl Robot { /// /// See [`new`](`Self::new`) to change behavior if realtime priority cannot be set. pub fn control_joint_positions< - F: FnMut(&RobotState, &Duration) -> JointPositions, + F: FnMut(&PandaState, &Duration) -> JointPositions, CM: Into>, L: Into>, CF: Into>, @@ -653,7 +679,7 @@ impl Robot { /// /// See [`new`](`Self::new`) to change behavior if realtime priority cannot be set. pub fn control_joint_velocities< - F: FnMut(&RobotState, &Duration) -> JointVelocities, + F: FnMut(&PandaState, &Duration) -> JointVelocities, CM: Into>, L: Into>, CF: Into>, @@ -696,7 +722,7 @@ impl Robot { /// /// See [`new`](`Self::new`) to change behavior if realtime priority cannot be set. pub fn control_cartesian_pose< - F: FnMut(&RobotState, &Duration) -> CartesianPose, + F: FnMut(&PandaState, &Duration) -> CartesianPose, CM: Into>, L: Into>, CF: Into>, @@ -739,7 +765,7 @@ impl Robot { /// /// See [`new`](`Self::new`) to change behavior if realtime priority cannot be set. pub fn control_cartesian_velocities< - F: FnMut(&RobotState, &Duration) -> CartesianVelocities, + F: FnMut(&PandaState, &Duration) -> CartesianVelocities, CM: Into>, L: Into>, CF: Into>, @@ -782,8 +808,8 @@ impl Robot { /// /// 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, - T: FnMut(&RobotState, &Duration) -> Torques, + F: FnMut(&PandaState, &Duration) -> JointVelocities, + T: FnMut(&PandaState, &Duration) -> Torques, L: Into>, CF: Into>, >( @@ -825,8 +851,8 @@ impl Robot { /// /// See [`new`](`Self::new`) to change behavior if realtime priority cannot be set. pub fn control_torques_and_joint_positions< - F: FnMut(&RobotState, &Duration) -> JointPositions, - T: FnMut(&RobotState, &Duration) -> Torques, + F: FnMut(&PandaState, &Duration) -> JointPositions, + T: FnMut(&PandaState, &Duration) -> Torques, L: Into>, CF: Into>, >( @@ -869,8 +895,8 @@ impl Robot { /// /// 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, - T: FnMut(&RobotState, &Duration) -> Torques, + F: FnMut(&PandaState, &Duration) -> CartesianPose, + T: FnMut(&PandaState, &Duration) -> Torques, L: Into>, CF: Into>, >( @@ -913,8 +939,8 @@ impl Robot { /// /// 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, - T: FnMut(&RobotState, &Duration) -> Torques, + F: FnMut(&PandaState, &Duration) -> CartesianVelocities, + T: FnMut(&PandaState, &Duration) -> Torques, L: Into>, CF: Into>, >( @@ -958,7 +984,7 @@ impl Robot { /// /// See [`new`](`Self::new`) to change behavior if real-time priority cannot be set. pub fn control_torques< - T: FnMut(&RobotState, &Duration) -> Torques, + T: FnMut(&PandaState, &Duration) -> Torques, L: Into>, CF: Into>, >( @@ -968,7 +994,7 @@ impl Robot { cutoff_frequency: CF, ) -> FrankaResult<()> { let motion_generator_callback = - |_state: &RobotState, _time_step: &Duration| JointVelocities::new([0.; 7]); + |_state: &PandaState, _time_step: &Duration| JointVelocities::new([0.; 7]); self.control_torques_intern( &motion_generator_callback, &mut control_callback, @@ -977,12 +1003,12 @@ impl Robot { ) } fn control_torques_intern< - F: FnMut(&RobotState, &Duration) -> U, + F: FnMut(&PandaState, &Duration) -> U, U: Finishable + Debug + MotionGeneratorTrait, >( &mut self, motion_generator_callback: F, - control_callback: &mut dyn FnMut(&RobotState, &Duration) -> Torques, + control_callback: &mut dyn FnMut(&PandaState, &Duration) -> Torques, limit_rate: Option, cutoff_frequency: Option, ) -> FrankaResult<()> { @@ -998,7 +1024,7 @@ impl Robot { control_loop.run() } fn control_motion_intern< - F: FnMut(&RobotState, &Duration) -> U, + F: FnMut(&PandaState, &Duration) -> U, U: Finishable + Debug + MotionGeneratorTrait, >( &mut self, @@ -1095,8 +1121,8 @@ mod tests { SetCollisionBehaviorRequest, SetCollisionBehaviorRequestWithHeader, SetCollisionBehaviorResponse, COMMAND_PORT, VERSION, }; - use crate::robot::types::RobotStateIntern; - use crate::{FrankaResult, JointPositions, MotionFinished, RealtimeConfig, Robot, RobotState}; + use crate::robot::types::PandaStateIntern; + use crate::{FrankaResult, JointPositions, MotionFinished, RealtimeConfig, Panda, PandaState}; use bincode::{deserialize, serialize, serialized_size}; use std::iter::FromIterator; use std::mem::size_of; @@ -1196,7 +1222,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; @@ -1341,7 +1367,7 @@ 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"); + let mut robot = Panda::new("127.0.0.1", None, None).expect("robot failure"); assert_eq!(robot.server_version(), VERSION); for (a, b, c, d, e, f, g, h) in collision_behavior_request_values.iter() { robot @@ -1412,7 +1438,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"); + Panda::new("127.0.0.1", RealtimeConfig::Ignore, None).expect("robot failure"); let mut counter = 0; let result = robot.control_joint_positions( |_, _| { @@ -1448,7 +1474,7 @@ mod tests { 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 { @@ -1477,7 +1503,7 @@ mod tests { { std::thread::sleep(Duration::from_secs_f64(0.01)); - let mut robot = Robot::new("127.0.0.1", None, None)?; + let mut robot = Panda::new("127.0.0.1", None, None)?; let _state = robot.read_once().unwrap(); } thread.join().unwrap(); @@ -1496,12 +1522,12 @@ mod tests { }); { std::thread::sleep(Duration::from_secs_f64(0.01)); - let mut robot = Robot::new("127.0.0.1", None, None)?; + let mut robot = Panda::new("127.0.0.1", None, None)?; let mut counter = 0; let mut first_time = true; let mut start_counter = 0; robot - .read(|state: &RobotState| { + .read(|state: &PandaState| { if first_time { first_time = false; counter = state.time.as_millis(); diff --git a/src/robot/control_loop.rs b/src/robot/control_loop.rs index b5ded97..7f83a9a 100644 --- a/src/robot/control_loop.rs +++ b/src/robot/control_loop.rs @@ -12,17 +12,17 @@ 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::robot_state::PandaState; use crate::robot::service_types::{MoveControllerMode, MoveDeviation}; use crate::robot::types::{ControllerCommand, MotionGeneratorCommand}; -type ControlCallback<'b> = &'b mut dyn FnMut(&RobotState, &Duration) -> Torques; +type ControlCallback<'b> = &'b mut dyn FnMut(&PandaState, &Duration) -> Torques; pub struct ControlLoop< 'a, 'b, T: RobotControl, U: Finishable + Debug + MotionGeneratorTrait, - F: FnMut(&RobotState, &Duration) -> U, + F: FnMut(&PandaState, &Duration) -> U, > { pub default_deviation: MoveDeviation, robot: &'a mut T, @@ -38,7 +38,7 @@ impl< 'b, T: RobotControl, U: Finishable + Debug + MotionGeneratorTrait, - F: FnMut(&RobotState, &Duration) -> U, + F: FnMut(&PandaState, &Duration) -> U, > ControlLoop<'a, 'b, T, U, F> { pub fn new( @@ -177,7 +177,7 @@ impl< fn spin_control( &mut self, - robot_state: &RobotState, + robot_state: &PandaState, time_step: &Duration, command: &mut ControllerCommand, ) -> bool { @@ -206,7 +206,7 @@ impl< } fn spin_motion( &mut self, - robot_state: &RobotState, + robot_state: &PandaState, time_step: &Duration, command: &mut MotionGeneratorCommand, ) -> bool { diff --git a/src/robot/control_types.rs b/src/robot/control_types.rs index 634be30..490e5a8 100644 --- a/src/robot/control_types.rs +++ b/src/robot/control_types.rs @@ -18,7 +18,7 @@ use crate::robot::rate_limiting::{ MAX_ROTATIONAL_ACCELERATION, MAX_ROTATIONAL_JERK, MAX_ROTATIONAL_VELOCITY, MAX_TRANSLATIONAL_ACCELERATION, MAX_TRANSLATIONAL_JERK, MAX_TRANSLATIONAL_VELOCITY, }; -use crate::robot::robot_state::RobotState; +use crate::robot::robot_state::PandaState; use crate::robot::service_types::MoveMotionGeneratorMode; use crate::robot::types::MotionGeneratorCommand; use crate::utils::Vector7; @@ -49,7 +49,7 @@ pub trait Finishable { /// converts the motion type to a MotionGeneratorCommand and applies rate limiting and filtering fn convert_motion( &self, - robot_state: &RobotState, + robot_state: &PandaState, command: &mut MotionGeneratorCommand, cutoff_frequency: f64, limit_rate: bool, @@ -107,7 +107,7 @@ impl Finishable for Torques { //todo pull convert motion out of the Finishable trait fn convert_motion( &self, - robot_state: &RobotState, + robot_state: &PandaState, command: &mut MotionGeneratorCommand, cutoff_frequency: f64, limit_rate: bool, @@ -153,7 +153,7 @@ impl Finishable for JointPositions { fn convert_motion( &self, - robot_state: &RobotState, + robot_state: &PandaState, command: &mut MotionGeneratorCommand, cutoff_frequency: f64, limit_rate: bool, @@ -227,7 +227,7 @@ impl Finishable for JointVelocities { fn convert_motion( &self, - robot_state: &RobotState, + robot_state: &PandaState, command: &mut MotionGeneratorCommand, cutoff_frequency: f64, limit_rate: bool, @@ -340,7 +340,7 @@ impl Finishable for CartesianPose { fn convert_motion( &self, - robot_state: &RobotState, + robot_state: &PandaState, command: &mut MotionGeneratorCommand, cutoff_frequency: f64, limit_rate: bool, @@ -461,7 +461,7 @@ impl Finishable for CartesianVelocities { fn convert_motion( &self, - robot_state: &RobotState, + robot_state: &PandaState, command: &mut MotionGeneratorCommand, cutoff_frequency: f64, limit_rate: bool, diff --git a/src/robot/logger.rs b/src/robot/logger.rs index db4fb76..b700074 100644 --- a/src/robot/logger.rs +++ b/src/robot/logger.rs @@ -5,7 +5,7 @@ use crate::robot::control_types::{ CartesianPose, CartesianVelocities, JointPositions, JointVelocities, Torques, }; -use crate::robot::robot_state::RobotState; +use crate::robot::robot_state::PandaState; use crate::robot::types::RobotCommand; use std::collections::VecDeque; @@ -34,7 +34,7 @@ pub struct RobotCommandLog { #[derive(Debug, Clone)] pub struct Record { /// Robot state of timestamp n+1. - pub state: RobotState, + pub state: PandaState, /// Robot command of timestamp n, after rate limiting (if activated). pub command: RobotCommandLog, } @@ -47,7 +47,7 @@ impl Record { } pub(crate) struct Logger { - states: VecDeque, + states: VecDeque, commands: VecDeque, ring_front: usize, ring_size: usize, @@ -64,7 +64,7 @@ impl Logger { log_size, } } - pub fn log(&mut self, state: &RobotState, command: &RobotCommand) { + pub fn log(&mut self, state: &PandaState, command: &RobotCommand) { self.states.push_back(state.clone()); self.commands.push_back(*command); self.ring_front = (self.ring_front + 1) % self.log_size; diff --git a/src/robot/robot_control.rs b/src/robot/robot_control.rs index 3be44fb..11d9969 100644 --- a/src/robot/robot_control.rs +++ b/src/robot/robot_control.rs @@ -2,7 +2,7 @@ // 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::PandaState; use crate::robot::service_types::{MoveControllerMode, MoveDeviation, MoveMotionGeneratorMode}; use crate::robot::types::{ControllerCommand, MotionGeneratorCommand}; @@ -25,11 +25,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: &PandaState, motion_id: u32, ) -> FrankaResult<()>; } diff --git a/src/robot/robot_impl.rs b/src/robot/robot_impl.rs index 1db48ad..db9964c 100644 --- a/src/robot/robot_impl.rs +++ b/src/robot/robot_impl.rs @@ -11,7 +11,7 @@ use crate::robot::errors::FrankaErrors; use crate::robot::logger::{Logger, Record}; use crate::robot::motion_generator_traits::MotionGeneratorTrait; use crate::robot::robot_control::RobotControl; -use crate::robot::robot_state::RobotState; +use crate::robot::robot_state::PandaState; use crate::robot::service_types::{ ConnectRequest, ConnectRequestWithHeader, ConnectResponse, ConnectStatus, MoveControllerMode, MoveDeviation, MoveMotionGeneratorMode, MoveRequest, MoveRequestWithHeader, MoveResponse, @@ -20,11 +20,33 @@ use crate::robot::service_types::{ }; use crate::robot::types::{ ControllerCommand, ControllerMode, MotionGeneratorCommand, MotionGeneratorMode, RobotCommand, - RobotMode, RobotStateIntern, + RobotMode, PandaStateIntern, }; use std::fs::remove_file; use std::path::Path; +pub trait RobotImplementation { + type State; + fn update2( + &mut self, + motion_command: Option<&MotionGeneratorCommand>, + control_command: Option<&ControllerCommand>, + ) -> FrankaResult; +} + +impl RobotImplementation for RobotImpl{ + type State = PandaState; + + fn update2(&mut self, motion_command: Option<&MotionGeneratorCommand>, control_command: Option<&ControllerCommand>) -> FrankaResult { + let robot_command = self.send_robot_command(motion_command, control_command)?; + let state = PandaState::from(self.receive_robot_state()?); + if let Some(command) = robot_command { + self.logger.log(&state, &command); + } + Ok(state) + } +} + pub struct RobotImpl { pub network: Network, logger: Logger, @@ -112,7 +134,7 @@ impl RobotImpl { robot_impl.connect_robot()?; let state = robot_impl .network - .udp_blocking_receive::() + .udp_blocking_receive::() .unwrap(); robot_impl.update_state_with(&state); Ok(robot_impl) @@ -139,18 +161,18 @@ impl RobotImpl { }), } } - fn update_state_with(&mut self, robot_state: &RobotStateIntern) { + fn update_state_with(&mut self, robot_state: &PandaStateIntern) { 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()?)) + pub fn read_once(&mut self) -> FrankaResult { + while self.network.udp_receive::().is_some() {} + Ok(PandaState::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::(); + 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 { @@ -160,7 +182,7 @@ impl RobotImpl { { latest_accepted_state = Some(received_state.unwrap()); } - received_state = self.network.udp_receive::(); + received_state = self.network.udp_receive::(); } while latest_accepted_state.is_none() { received_state = Some(self.network.udp_blocking_receive()?); @@ -417,9 +439,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 = PandaState::from(self.receive_robot_state()?); if let Some(command) = robot_command { self.logger.log(&state, &command); } @@ -432,7 +454,7 @@ impl RobotControl for RobotImpl { fn throw_on_motion_error( &mut self, - robot_state: &RobotState, + robot_state: &PandaState, motion_id: u32, ) -> FrankaResult<()> { if robot_state.robot_mode != RobotMode::Move diff --git a/src/robot/robot_state.rs b/src/robot/robot_state.rs index 4b4d1d3..2f09a40 100644 --- a/src/robot/robot_state.rs +++ b/src/robot/robot_state.rs @@ -5,13 +5,13 @@ use std::time::Duration; use crate::robot::errors::{FrankaErrorKind, FrankaErrors}; -use crate::robot::types::{RobotMode, RobotStateIntern}; +use crate::robot::types::{RobotMode, PandaStateIntern}; use nalgebra::{Matrix3, Vector3}; /// Describes the robot state. #[derive(Debug, Clone, Default)] #[allow(non_snake_case)] -pub struct RobotState { +pub struct PandaState { /// ![^{O}T_{EE}](https://latex.codecogs.com/png.latex?^{O}T_{EE}) /// /// Measured end effector pose in base frame. @@ -254,9 +254,9 @@ pub struct RobotState { /// instead pub time: Duration, } -impl From for RobotState { +impl From for PandaState { #[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; @@ -319,7 +319,7 @@ impl From for RobotState { let current_errors = FrankaErrors::new(robot_state.errors, FrankaErrorKind::Error); let last_motion_errors = FrankaErrors::new(robot_state.errors, FrankaErrorKind::ReflexReason); - RobotState { + PandaState { O_T_EE, O_T_EE_d, F_T_EE, diff --git a/src/robot/types.rs b/src/robot/types.rs index db4301f..efd7453 100644 --- a/src/robot/types.rs +++ b/src/robot/types.rs @@ -46,7 +46,7 @@ impl Default for RobotMode { #[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 +96,9 @@ pub struct RobotStateIntern { pub control_command_success_rate: f64, } -impl RobotStateIntern { +impl PandaStateIntern { pub fn dummy() -> Self { - RobotStateIntern { + PandaStateIntern { message_id: 0, O_T_EE: [0.; 16], O_T_EE_d: [0.; 16], diff --git a/src/utils.rs b/src/utils.rs index a6136ae..29488bd 100644 --- a/src/utils.rs +++ b/src/utils.rs @@ -3,7 +3,7 @@ //! contains useful type definitions and conversion functions. use crate::robot::control_types::{Finishable, JointPositions}; -use crate::robot::robot_state::RobotState; +use crate::robot::robot_state::PandaState; use nalgebra::{Isometry3, Matrix4, MatrixMN, MatrixN, Rotation3, Vector3, VectorN, U6, U7}; use std::time::Duration; @@ -171,7 +171,7 @@ impl MotionGenerator { /// Joint positions for use inside a control loop. pub fn generate_motion( &mut self, - robot_state: &RobotState, + robot_state: &PandaState, period: &Duration, ) -> JointPositions { self.time += period.as_secs_f64(); @@ -192,7 +192,7 @@ impl MotionGenerator { #[cfg(test)] mod test { - use crate::{array_to_isometry, Finishable, MotionGenerator, RobotState}; + use crate::{array_to_isometry, Finishable, MotionGenerator, PandaState}; use nalgebra::Rotation3; use std::time::Duration; @@ -302,7 +302,7 @@ mod test { ], [1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0], ]; - let mut state = RobotState::default(); + let mut state = PandaState::default(); let mut motion_generator = MotionGenerator::new(1.0, &[1.; 7]); let mut joint_pos = motion_generator.generate_motion(&state, &Duration::from_secs_f64(0.0)); slice_compare(&joint_pos.q, &q_des[0], 1e-10); From cbe0618d439ccf00b4dc98941247efadbd8dc27c Mon Sep 17 00:00:00 2001 From: Marco Boneberger Date: Fri, 9 Sep 2022 16:43:25 +0200 Subject: [PATCH 02/63] proof of concept. Can use either FR3 or Panda with generate_cartesian_pose_motion example TODO: * make the logger compatible * get rid of code duplication * Make FR3 actually use FR3 data type * Fix warnings and lints * make other examples work * Fix documentation --- examples/generate_cartesian_pose_motion.rs | 18 +- src/gripper.rs | 2 +- src/robot.rs | 400 ++++++++++++------ src/robot/control_loop.rs | 49 ++- src/robot/control_types.rs | 332 +++++++++++++-- src/robot/robot_control.rs | 7 +- src/robot/robot_impl.rs | 462 ++++++++++++++++++++- src/robot/robot_state.rs | 390 +++++++++++++++++ src/robot/types.rs | 2 + src/utils.rs | 5 +- 10 files changed, 1471 insertions(+), 196 deletions(-) diff --git a/examples/generate_cartesian_pose_motion.rs b/examples/generate_cartesian_pose_motion.rs index 12d0e4e..6f44562 100644 --- a/examples/generate_cartesian_pose_motion.rs +++ b/examples/generate_cartesian_pose_motion.rs @@ -2,12 +2,14 @@ // 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 franka::{ConvertMotion, Finishable, FrankaResult, Panda}; +// use franka::Robot; +use franka::PandaState; +use franka::{CartesianPose}; use std::f64::consts::PI; use std::time::Duration; +use franka::robot::{FR3, Robot}; +use franka::robot::robot_state::FR3State; /// An example showing how to generate a Cartesian motion. /// @@ -21,8 +23,8 @@ struct CommandLineArguments { fn main() -> FrankaResult<()> { let address = CommandLineArguments::parse(); - let mut robot = Robot::new(address.franka_ip.as_str(), None, None)?; - robot.set_default_behavior()?; + let mut robot = FR3::new(address.franka_ip.as_str(), 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(); @@ -40,11 +42,11 @@ fn main() -> FrankaResult<()> { )?; let q_goal = [0., -PI / 4., 0., -3. * PI / 4., 0., PI / 2., PI / 4.]; - robot.joint_motion(0.5, &q_goal)?; + // robot.joint_motion(0.5, &q_goal)?; println!("Finished moving to initial joint configuration."); let mut initial_pose = CartesianPose::new([0.0; 16], None); let mut time = 0.; - let callback = |state: &RobotState, time_step: &Duration| -> CartesianPose { + let callback = |state: &FR3State, time_step: &Duration| -> CartesianPose { time += time_step.as_secs_f64(); if time == 0. { initial_pose.O_T_EE = state.O_T_EE_c; diff --git a/src/gripper.rs b/src/gripper.rs index ae99df1..b19ac55 100644 --- a/src/gripper.rs +++ b/src/gripper.rs @@ -44,7 +44,7 @@ impl Gripper { gripper.connect_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 diff --git a/src/robot.rs b/src/robot.rs index 01711af..6515cf6 100644 --- a/src/robot.rs +++ b/src/robot.rs @@ -12,13 +12,13 @@ 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, + CartesianPose, CartesianVelocities, ControllerMode, ConvertMotion, 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::robot::robot_control::RobotControl; -use crate::robot::robot_impl::{RobotImpl, RobotImplementation}; +use crate::robot::robot_impl::{RobotImplPanda, RobotImplementation, RobotImplFR3}; use crate::robot::service_types::{ AutomaticErrorRecoveryRequestWithHeader, AutomaticErrorRecoveryResponse, AutomaticErrorRecoveryStatus, GetCartesianLimitRequest, GetCartesianLimitRequestWithHeader, @@ -35,6 +35,8 @@ use crate::robot::service_types::{ use crate::robot::virtual_wall_cuboid::VirtualWallCuboid; use crate::utils::MotionGenerator; use robot_state::PandaState; +use crate::Finishable; +use crate::robot::robot_state::{FR3State, RobotState}; mod control_loop; mod control_tools; @@ -52,11 +54,12 @@ pub(crate) mod service_types; pub(crate) mod types; pub mod virtual_wall_cuboid; -pub trait Robot { - type State:Debug; - type Rob:RobotImplementation; +pub trait Robot where CartesianPose: ConvertMotion<::State1>{ + type State1:RobotState; + type Rob:RobotImplementation; fn get_rob(&mut self) -> &mut Self::Rob; - fn read bool>(&mut self, + fn get_net(&mut self) -> &mut Network; + fn read bool>(&mut self, mut read_callback: F, ) -> FrankaResult<()> { loop { @@ -68,6 +71,119 @@ pub trait Robot { } Ok(()) } + fn read_once(&mut self) -> FrankaResult { + self.get_rob().read_once2() + } + + #[allow(clippy::too_many_arguments)] + fn set_collision_behavior( + &mut self, + 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 command = SetCollisionBehaviorRequestWithHeader { + header: self.get_net().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.get_net().tcp_send_request(command); + let response: SetCollisionBehaviorResponse = self + .get_net() + .tcp_blocking_receive_response(command_id); + handle_getter_setter_status(response.status) + } + + #[allow(non_snake_case)] + fn set_joint_impedance(&mut self, K_theta: [f64; 7]) -> FrankaResult<()> { + let command = SetJointImpedanceRequestWithHeader { + header: self.get_net().create_header_for_robot( + RobotCommandEnum::SetJointImpedance, + size_of::(), + ), + request: SetJointImpedanceRequest::new(K_theta), + }; + let command_id: u32 = self.get_net().tcp_send_request(command); + let response: SetJointImpedanceResponse = self + .get_net() + .tcp_blocking_receive_response(command_id); + handle_getter_setter_status(response.status) + } + + #[allow(non_snake_case)] + fn set_cartesian_impedance(&mut self, K_x: [f64; 6]) -> FrankaResult<()> { + let command = SetCartesianImpedanceRequestWithHeader { + header: self.get_net().create_header_for_robot( + RobotCommandEnum::SetCartesianImpedance, + size_of::(), + ), + request: SetCartesianImpedanceRequest::new(K_x), + }; + let command_id: u32 = self.get_net().tcp_send_request(command); + let response: SetCartesianImpedanceResponse = self + .get_net() + .tcp_blocking_receive_response(command_id); + handle_getter_setter_status(response.status) + } + + fn control_motion_intern< + F: FnMut(&Self::State1, &Duration) -> U, + U: ConvertMotion + 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(true); + let cutoff_frequency = cutoff_frequency.unwrap_or(DEFAULT_CUTOFF_FREQUENCY); + let mut control_loop = ControlLoop::from_control_mode( + self.get_rob(), + controller_mode, + motion_generator_callback, + limit_rate, + cutoff_frequency, + )?; + control_loop.run() + } + + fn control_cartesian_pose< + F: FnMut(&Self::State1, &Duration) -> CartesianPose, + CM: Into>, + L: Into>, + CF: Into>, + >( + &mut self, + motion_generator_callback: F, + controller_mode: CM, + limit_rate: L, + cutoff_frequency: CF, + ) -> FrankaResult<()> { + self.control_motion_intern( + motion_generator_callback, + controller_mode.into(), + limit_rate.into(), + cutoff_frequency.into(), + ) + } } /// Maintains a network connection to the robot, provides the current robot state, gives access to @@ -124,17 +240,57 @@ pub trait Robot { /// 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 Panda { - robimpl: RobotImpl, + robimpl: RobotImplPanda, } impl Robot for Panda { - type State = PandaState; - type Rob = RobotImpl; + type State1 = PandaState; + type Rob = RobotImplPanda; + fn get_net(&mut self) -> &mut Network { + &mut self.robimpl.network + } + fn get_rob(&mut self) ->&mut Self::Rob { + &mut self.robimpl + } +} + +pub struct FR3 { + robimpl: RobotImplFR3, +} + +impl Robot for FR3 { + type State1 = FR3State; + type Rob = RobotImplFR3; + fn get_net(&mut self) -> &mut Network { + &mut self.robimpl.network + } fn get_rob(&mut self) ->&mut Self::Rob { &mut self.robimpl } } +impl 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( + NetworkType::Robot, + franka_address, + service_types::COMMAND_PORT, + ) + .map_err(|_| FrankaException::NetworkException { + message: "Connection could not be established".to_string(), + })?; + Ok(FR3 { + robimpl: RobotImplFR3::new(network, log_size, realtime_config)?, + }) + } +} + impl Panda { /// Establishes a connection with the robot. /// @@ -174,7 +330,7 @@ impl Panda { message: "Connection could not be established".to_string(), })?; Ok(Panda { - robimpl: RobotImpl::new(network, log_size, realtime_config)?, + robimpl: RobotImplPanda::new(network, log_size, realtime_config)?, }) } /// Starts a loop for reading the current robot state. @@ -219,9 +375,9 @@ impl Panda { /// * [`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() - } + // pub fn read_once(&mut self) -> FrankaResult { + // self.robimpl.read_once() + // } /// Changes the collision behavior. /// @@ -254,41 +410,41 @@ impl Panda { /// * [`RobotState::joint_contact`](`crate::RobotState::joint_contact`) /// * [`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( - &mut self, - 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 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) - } + // #[allow(clippy::too_many_arguments)] + // pub fn set_collision_behavior( + // &mut self, + // 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 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) + // } /// Sets the impedance for each joint in the internal controller. /// @@ -298,22 +454,22 @@ impl Panda { /// # 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_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) - } + // #[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) + // } /// Sets the Cartesian impedance for (x, y, z, roll, pitch, yaw) in the internal controller. /// @@ -325,22 +481,22 @@ impl Panda { /// # 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_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) - } + // #[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) + // } /// Locks or unlocks guiding mode movement in (x, y, z, roll, pitch, yaw). /// /// If a flag is set to true, movement is unlocked. @@ -721,25 +877,25 @@ impl Panda { /// * 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< - F: FnMut(&PandaState, &Duration) -> CartesianPose, - CM: Into>, - L: Into>, - CF: Into>, - >( - &mut self, - motion_generator_callback: F, - controller_mode: CM, - limit_rate: L, - cutoff_frequency: CF, - ) -> FrankaResult<()> { - self.control_motion_intern( - motion_generator_callback, - controller_mode.into(), - limit_rate.into(), - cutoff_frequency.into(), - ) - } + // pub fn control_cartesian_pose< + // F: FnMut(&PandaState, &Duration) -> CartesianPose, + // CM: Into>, + // L: Into>, + // CF: Into>, + // >( + // &mut self, + // motion_generator_callback: F, + // controller_mode: CM, + // limit_rate: L, + // cutoff_frequency: CF, + // ) -> FrankaResult<()> { + // self.control_motion_intern( + // motion_generator_callback, + // 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. @@ -1004,7 +1160,7 @@ impl Panda { } fn control_torques_intern< F: FnMut(&PandaState, &Duration) -> U, - U: Finishable + Debug + MotionGeneratorTrait, + U: ConvertMotion + Debug + MotionGeneratorTrait + Finishable, >( &mut self, motion_generator_callback: F, @@ -1023,28 +1179,28 @@ impl Panda { )?; control_loop.run() } - fn control_motion_intern< - F: FnMut(&PandaState, &Duration) -> U, - U: Finishable + Debug + MotionGeneratorTrait, - >( - &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(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() - } + // fn control_motion_intern< + // F: FnMut(&PandaState, &Duration) -> U, + // U: Finishable + Debug + MotionGeneratorTrait, + // >( + // &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(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() + // } /// Sets a default collision behavior, joint impedance and Cartesian impedance. /// # Errors /// * [`CommandException`](`crate::exception::FrankaException::CommandException`) if the Control reports an error. diff --git a/src/robot/control_loop.rs b/src/robot/control_loop.rs index 7f83a9a..11ff371 100644 --- a/src/robot/control_loop.rs +++ b/src/robot/control_loop.rs @@ -4,30 +4,32 @@ use std::fmt::Debug; use std::time::Duration; use crate::exception::{FrankaException, FrankaResult}; +use crate::Finishable; 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::PandaState; +use crate::robot::robot_impl::RobotImplementation; +use crate::robot::robot_state::{PandaState, RobotState}; use crate::robot::service_types::{MoveControllerMode, MoveDeviation}; use crate::robot::types::{ControllerCommand, MotionGeneratorCommand}; -type ControlCallback<'b> = &'b mut dyn FnMut(&PandaState, &Duration) -> Torques; +type ControlCallback<'b,State> = &'b mut dyn FnMut(&State, &Duration) -> Torques; pub struct ControlLoop< 'a, 'b, - T: RobotControl, - U: Finishable + Debug + MotionGeneratorTrait, - F: FnMut(&PandaState, &Duration) -> U, + T: RobotImplementation, + U: ConvertMotion + Debug + MotionGeneratorTrait + Finishable, + F: FnMut(&T::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 +38,14 @@ pub struct ControlLoop< impl< 'a, 'b, - T: RobotControl, - U: Finishable + Debug + MotionGeneratorTrait, - F: FnMut(&PandaState, &Duration) -> U, + T: RobotImplementation, + U: ConvertMotion + Debug + MotionGeneratorTrait + Finishable, + F: FnMut(&T::State, &Duration) -> U, > ControlLoop<'a, 'b, T, U, F> { pub fn new( robot: &'a mut T, - control_callback: ControlCallback<'b>, + control_callback: ControlCallback<'b,T::State>, motion_callback: F, limit_rate: bool, cutoff_frequency: f64, @@ -87,7 +89,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 +134,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 +163,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 +179,7 @@ impl< fn spin_control( &mut self, - robot_state: &PandaState, + robot_state: &T::State, time_step: &Duration, command: &mut ControllerCommand, ) -> bool { @@ -188,7 +190,7 @@ 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, ); } @@ -197,16 +199,17 @@ impl< control_output.tau_J = limit_rate_torques( &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; command.tau_J_d.iter().for_each(|x| assert!(x.is_finite())); - !control_output.is_finished() + // !control_output.is_finished() + true } fn spin_motion( &mut self, - robot_state: &PandaState, + robot_state: &T::State2, time_step: &Duration, command: &mut MotionGeneratorCommand, ) -> bool { diff --git a/src/robot/control_types.rs b/src/robot/control_types.rs index 490e5a8..cf4dd6d 100644 --- a/src/robot/control_types.rs +++ b/src/robot/control_types.rs @@ -18,11 +18,12 @@ use crate::robot::rate_limiting::{ MAX_ROTATIONAL_ACCELERATION, MAX_ROTATIONAL_JERK, MAX_ROTATIONAL_VELOCITY, MAX_TRANSLATIONAL_ACCELERATION, MAX_TRANSLATIONAL_JERK, MAX_TRANSLATIONAL_VELOCITY, }; -use crate::robot::robot_state::PandaState; +use crate::robot::robot_state::{FR3State, PandaState, RobotState}; use crate::robot::service_types::MoveMotionGeneratorMode; use crate::robot::types::MotionGeneratorCommand; use crate::utils::Vector7; use nalgebra::{Isometry3, Vector6}; +use crate::robot::robot_impl::RobotImplFR3; /// Available controller modes for a [`Robot`](`crate::Robot`) pub enum ControllerMode { @@ -41,33 +42,65 @@ pub enum RealtimeConfig { /// Helper type for control and motion generation loops. /// /// Used to determine whether to terminate a loop after the control callback has returned. -pub trait Finishable { - /// Determines whether to finish a currently running motion. - 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); +pub trait ConvertMotion { + /// converts the motion type to a MotionGeneratorCommand and applies rate limiting and filtering fn convert_motion( &self, - robot_state: &PandaState, + robot_state: &State3, command: &mut MotionGeneratorCommand, cutoff_frequency: f64, limit_rate: bool, ); } -/// A trait for a Finshable control type to finish the motion -pub trait MotionFinished { - /// Helper method to indicate that a motion should stop after processing the given command. +pub trait Finishable { + /// Determines whether to finish a currently running motion. + 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); fn motion_finished(self) -> Self; } -impl MotionFinished for T { - fn motion_finished(mut self) -> Self { - self.set_motion_finished(true); - self - } -} +// /// A trait for a Finshable control type to finish the motion +// pub trait MotionFinished : Finishable { +// /// Helper method to indicate that a motion should stop after processing the given command. +// fn motion_finished(self) -> Self; +// } +// +// // impl + Copy> MotionFinished for T { +// // type State4 = PandaState ; +// // +// // fn motion_finished(mut self) -> Self { +// // self.set_motion_finished(true); +// // self +// // } +// // } +// +// impl + Copy> MotionFinished for T { +// +// fn motion_finished(mut self) -> Self { +// self.set_motion_finished(true); +// self +// } +// } +// +// impl + Copy> MotionFinished for T { +// +// fn motion_finished(mut self) -> Self { +// self.set_motion_finished(true); +// self +// } +// } + +// impl + Copy,X> MotionFinished for T { +// type State4 = FR3State ; +// +// fn motion_finished(mut self) -> Self { +// self.set_motion_finished(true); +// self +// } +// } /// Stores joint-level torque commands without gravity and friction. #[derive(Serialize, Deserialize, Debug, Copy, Clone)] @@ -96,18 +129,27 @@ impl Torques { } } -impl Finishable for Torques { +impl Finishable for Torques{ fn is_finished(&self) -> bool { self.motion_finished } fn set_motion_finished(&mut self, finished: bool) { self.motion_finished = finished; } + fn motion_finished(mut self) -> Self { + self.set_motion_finished(true); + self + } +} + +impl ConvertMotion for Torques { + + #[allow(unused_variables)] //todo pull convert motion out of the Finishable trait fn convert_motion( &self, - robot_state: &PandaState, + robot_state: &Statee, command: &mut MotionGeneratorCommand, cutoff_frequency: f64, limit_rate: bool, @@ -115,6 +157,26 @@ impl Finishable for Torques { unimplemented!() } } +// impl Finishable for Torques { +// +// fn is_finished(&self) -> bool { +// self.motion_finished +// } +// 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: &Self::State3, +// command: &mut MotionGeneratorCommand, +// cutoff_frequency: f64, +// limit_rate: bool, +// ) { +// unimplemented!() +// } +// } /// Stores values for joint position motion generation. #[derive(Serialize, Deserialize, Debug, Copy, Clone)] @@ -142,14 +204,18 @@ impl JointPositions { } } } - -impl Finishable for JointPositions { +impl Finishable for JointPositions{ fn is_finished(&self) -> bool { self.motion_finished } fn set_motion_finished(&mut self, finished: bool) { self.motion_finished = finished; } + fn motion_finished(mut self) -> Self { + self.set_motion_finished(true); + self + }} +impl ConvertMotion for JointPositions { fn convert_motion( &self, @@ -183,6 +249,39 @@ impl Finishable for JointPositions { command.q_c.iter().for_each(|x| assert!(x.is_finite())); } } +impl ConvertMotion for JointPositions { + fn convert_motion( + &self, + robot_state: &FR3State, + command: &mut MotionGeneratorCommand, + cutoff_frequency: f64, + limit_rate: bool, + ) { + command.q_c = self.q; + if cutoff_frequency < MAX_CUTOFF_FREQUENCY { + for i in 0..7 { + command.q_c[i] = low_pass_filter( + DELTA_T, + command.q_c[i], + robot_state.q_d[i], + cutoff_frequency, + ); + } + } + if limit_rate { + command.q_c = limit_rate_joint_positions( + &MAX_JOINT_VELOCITY, + &MAX_JOINT_ACCELERATION, + &MAX_JOINT_JERK, + &command.q_c, + &robot_state.q_d, + &robot_state.dq_d, + &robot_state.ddq_d, + ); + } + command.q_c.iter().for_each(|x| assert!(x.is_finite())); + } +} impl MotionGeneratorTrait for JointPositions { fn get_motion_generator_mode() -> MoveMotionGeneratorMode { @@ -216,14 +315,19 @@ impl JointVelocities { } } } - -impl Finishable for JointVelocities { +impl Finishable for JointVelocities{ fn is_finished(&self) -> bool { self.motion_finished } fn set_motion_finished(&mut self, finished: bool) { self.motion_finished = finished; } + fn motion_finished(mut self) -> Self { + self.set_motion_finished(true); + self + } +} +impl ConvertMotion for JointVelocities { fn convert_motion( &self, @@ -257,6 +361,39 @@ impl Finishable for JointVelocities { } } +impl ConvertMotion for JointVelocities { + fn convert_motion( + &self, + robot_state: &FR3State, + command: &mut MotionGeneratorCommand, + cutoff_frequency: f64, + limit_rate: bool, + ) { + command.dq_c = self.dq; + if cutoff_frequency < MAX_CUTOFF_FREQUENCY { + for i in 0..7 { + command.dq_c[i] = low_pass_filter( + DELTA_T, + command.dq_c[i], + robot_state.dq_d[i], + cutoff_frequency, + ); + } + } + if limit_rate { + command.dq_c = limit_rate_joint_velocities( + &MAX_JOINT_VELOCITY, + &MAX_JOINT_ACCELERATION, + &MAX_JOINT_JERK, + &command.dq_c, + &robot_state.dq_d, + &robot_state.ddq_d, + ); + } + command.dq_c.iter().for_each(|x| assert!(x.is_finite())); + } +} + impl MotionGeneratorTrait for JointVelocities { fn get_motion_generator_mode() -> MoveMotionGeneratorMode { MoveMotionGeneratorMode::JointVelocity @@ -330,13 +467,20 @@ impl CartesianPose { } } -impl Finishable for CartesianPose { +impl Finishable for CartesianPose{ fn is_finished(&self) -> bool { self.motion_finished } fn set_motion_finished(&mut self, finished: bool) { self.motion_finished = finished; } + fn motion_finished(mut self) -> Self { + self.set_motion_finished(true); + self + } +} + +impl ConvertMotion for CartesianPose { fn convert_motion( &self, @@ -401,6 +545,71 @@ impl Finishable for CartesianPose { } } +impl ConvertMotion for CartesianPose { + + fn convert_motion( + &self, + robot_state: &FR3State, + command: &mut MotionGeneratorCommand, + cutoff_frequency: f64, + limit_rate: bool, + ) { + command.O_T_EE_c = self.O_T_EE; + if cutoff_frequency < MAX_CUTOFF_FREQUENCY { + command.O_T_EE_c = cartesian_low_pass_filter( + DELTA_T, + &command.O_T_EE_c, + &robot_state.O_T_EE_c, + cutoff_frequency, + ); + } + + 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, + &command.O_T_EE_c, + &robot_state.O_T_EE_c, + &robot_state.O_dP_EE_c, + &robot_state.O_ddP_EE_c, + ); + } + check_matrix(&command.O_T_EE_c); + + if self.has_elbow() { + command.valid_elbow = true; + command.elbow_c = self.elbow.unwrap(); + if cutoff_frequency < MAX_CUTOFF_FREQUENCY { + command.elbow_c[0] = low_pass_filter( + DELTA_T, + command.elbow_c[0], + robot_state.elbow_c[0], + cutoff_frequency, + ); + } + if limit_rate { + command.elbow_c[0] = limit_rate_position( + MAX_ELBOW_VELOCITY, + MAX_ELBOW_ACCELERATION, + MAX_ELBOW_JERK, + command.elbow_c[0], + robot_state.elbow_c[0], + robot_state.delbow_c[0], + robot_state.ddelbow_c[0], + ); + } + CartesianPose::check_elbow(&command.elbow_c); + } else { + command.valid_elbow = false; + command.elbow_c = [0.; 2]; + } + } +} + impl MotionGeneratorTrait for CartesianPose { fn get_motion_generator_mode() -> MoveMotionGeneratorMode { MoveMotionGeneratorMode::CartesianPosition @@ -450,14 +659,19 @@ impl CartesianVelocities { self.elbow.is_some() } } - -impl Finishable for CartesianVelocities { +impl Finishable for CartesianVelocities{ fn is_finished(&self) -> bool { self.motion_finished } fn set_motion_finished(&mut self, finished: bool) { self.motion_finished = finished; } + fn motion_finished(mut self) -> Self { + self.set_motion_finished(true); + self + } +} +impl ConvertMotion for CartesianVelocities { fn convert_motion( &self, @@ -525,6 +739,74 @@ impl Finishable for CartesianVelocities { } } +impl ConvertMotion for CartesianVelocities { + + fn convert_motion( + &self, + robot_state: &FR3State, + command: &mut MotionGeneratorCommand, + cutoff_frequency: f64, + limit_rate: bool, + ) { + command.O_dP_EE_c = self.O_dP_EE; + if cutoff_frequency < MAX_CUTOFF_FREQUENCY { + for i in 0..6 { + command.O_dP_EE_c[i] = low_pass_filter( + DELTA_T, + command.O_dP_EE_c[i], + robot_state.O_dP_EE_c[i], + cutoff_frequency, + ); + } + } + 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, + &command.O_dP_EE_c, + &robot_state.O_dP_EE_c, + &robot_state.O_ddP_EE_c, + ); + } + command + .O_dP_EE_c + .iter() + .for_each(|x| assert!(x.is_finite())); + + if self.has_elbow() { + command.valid_elbow = true; + command.elbow_c = self.elbow.unwrap(); + if cutoff_frequency < MAX_CUTOFF_FREQUENCY { + command.elbow_c[0] = low_pass_filter( + DELTA_T, + command.elbow_c[0], + robot_state.elbow_c[0], + cutoff_frequency, + ); + } + if limit_rate { + command.elbow_c[0] = limit_rate_position( + MAX_ELBOW_VELOCITY, + MAX_ELBOW_ACCELERATION, + MAX_ELBOW_JERK, + command.elbow_c[0], + robot_state.elbow_c[0], + robot_state.delbow_c[0], + robot_state.ddelbow_c[0], + ); + } + CartesianPose::check_elbow(&command.elbow_c); + } else { + command.valid_elbow = false; + command.elbow_c = [0.; 2]; + } + } +} + impl MotionGeneratorTrait for CartesianVelocities { fn get_motion_generator_mode() -> MoveMotionGeneratorMode { MoveMotionGeneratorMode::CartesianVelocity diff --git a/src/robot/robot_control.rs b/src/robot/robot_control.rs index 11d9969..15020eb 100644 --- a/src/robot/robot_control.rs +++ b/src/robot/robot_control.rs @@ -2,11 +2,12 @@ // Licensed under the EUPL-1.2-or-later use crate::exception::FrankaResult; use crate::robot::control_types::RealtimeConfig; -use crate::robot::robot_state::PandaState; +use crate::robot::robot_state::{PandaState, RobotState}; use crate::robot::service_types::{MoveControllerMode, MoveDeviation, MoveMotionGeneratorMode}; use crate::robot::types::{ControllerCommand, MotionGeneratorCommand}; pub trait RobotControl { + type State2:RobotState; fn start_motion( &mut self, controller_mode: MoveControllerMode, @@ -25,11 +26,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: &PandaState, + robot_state: &Self::State2, motion_id: u32, ) -> FrankaResult<()>; } diff --git a/src/robot/robot_impl.rs b/src/robot/robot_impl.rs index db9964c..f5c4f98 100644 --- a/src/robot/robot_impl.rs +++ b/src/robot/robot_impl.rs @@ -9,9 +9,9 @@ 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::robot_control::RobotControl; -use crate::robot::robot_state::PandaState; +use crate::robot::robot_state::{FR3State, PandaState, RobotState}; use crate::robot::service_types::{ ConnectRequest, ConnectRequestWithHeader, ConnectResponse, ConnectStatus, MoveControllerMode, MoveDeviation, MoveMotionGeneratorMode, MoveRequest, MoveRequestWithHeader, MoveResponse, @@ -25,16 +25,19 @@ use crate::robot::types::{ use std::fs::remove_file; use std::path::Path; -pub trait RobotImplementation { - type State; +pub trait RobotImplementation : RobotControl { + type State:RobotState; fn update2( &mut self, motion_command: Option<&MotionGeneratorCommand>, control_command: Option<&ControllerCommand>, ) -> FrankaResult; + fn read_once2(&mut self) -> FrankaResult; + fn load_model2(&mut self, persistent: bool) -> FrankaResult; + fn server_version2(&self) -> u16; } -impl RobotImplementation for RobotImpl{ +impl RobotImplementation for RobotImplPanda { type State = PandaState; fn update2(&mut self, motion_command: Option<&MotionGeneratorCommand>, control_command: Option<&ControllerCommand>) -> FrankaResult { @@ -45,9 +48,74 @@ impl RobotImplementation for RobotImpl{ } Ok(state) } + + fn read_once2(&mut self) -> FrankaResult { + while self.network.udp_receive::().is_some() {} + Ok(PandaState::from(self.receive_robot_state()?)) + } + fn load_model2(&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) + } + fn server_version2(&self) -> u16 { + self.ri_version.unwrap() + } +} + +impl RobotImplementation for RobotImplFR3 { + type State = FR3State; + + fn update2(&mut self, motion_command: Option<&MotionGeneratorCommand>, control_command: Option<&ControllerCommand>) -> FrankaResult { + let robot_command = self.send_robot_command(motion_command, control_command)?; + let state = FR3State::from(self.receive_robot_state()?); + if let Some(command) = robot_command { + // self.logger.log(&state, &command); // todo fix logger + } + Ok(state) + } + + fn read_once2(&mut self) -> FrankaResult { + while self.network.udp_receive::().is_some() {} + Ok(FR3State::from(self.receive_robot_state()?)) + } + fn load_model2(&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) + } + fn server_version2(&self) -> u16 { + self.ri_version.unwrap() + } } -pub struct RobotImpl { +pub struct RobotImplPanda { + pub network: Network, + logger: Logger, + realtime_config: RealtimeConfig, + ri_version: Option, + motion_generator_mode: Option, + current_move_motion_generator_mode: MotionGeneratorMode, + controller_mode: ControllerMode, + current_move_controller_mode: Option, + message_id: u64, +} + +pub struct RobotImplFR3 { pub network: Network, logger: Logger, realtime_config: RealtimeConfig, @@ -111,7 +179,7 @@ fn handle_command_response_stop(response: &StopMoveResponse) -> Result<(), Frank } } -impl RobotImpl { +impl RobotImplPanda { pub fn new( network: Network, log_size: usize, @@ -120,7 +188,7 @@ impl RobotImpl { let current_move_generator_mode = MotionGeneratorMode::Idle; let controller_mode = ControllerMode::Other; let logger = Logger::new(log_size); - let mut robot_impl = RobotImpl { + let mut robot_impl = RobotImplPanda { network, logger, realtime_config, @@ -305,7 +373,203 @@ impl RobotImpl { } } -impl RobotControl for RobotImpl { +impl RobotImplFR3 { + 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 = RobotImplFR3 { + 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: &PandaStateIntern) { + 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(FR3State::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), + }), + }; + } + } + + 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 RobotImplPanda { + type State2 = PandaState; + fn start_motion( &mut self, controller_mode: MoveControllerMode, @@ -477,12 +741,186 @@ impl RobotControl for RobotImpl { } } -impl MotionGeneratorTrait for RobotImpl { - fn get_motion_generator_mode() -> MoveMotionGeneratorMode { - MoveMotionGeneratorMode::JointVelocity +impl RobotControl for RobotImplFR3 { + type State2 = FR3State; + + fn start_motion( + &mut self, + controller_mode: MoveControllerMode, + motion_generator_mode: MoveMotionGeneratorMode, + maximum_path_deviation: &MoveDeviation, + maximum_goal_deviation: &MoveDeviation, + ) -> FrankaResult { + if self.motion_generator_running() || self.controller_running() { + panic!("libfranka-rs robot: Attempted to start multiple motions!"); + } + self.current_move_motion_generator_mode = match motion_generator_mode { + MoveMotionGeneratorMode::JointPosition => MotionGeneratorMode::JointPosition, + MoveMotionGeneratorMode::JointVelocity => MotionGeneratorMode::JointVelocity, + MoveMotionGeneratorMode::CartesianPosition => MotionGeneratorMode::CartesianPosition, + MoveMotionGeneratorMode::CartesianVelocity => MotionGeneratorMode::CartesianVelocity, + }; + self.current_move_controller_mode = match controller_mode { + MoveControllerMode::JointImpedance => Some(ControllerMode::JointImpedance), + MoveControllerMode::ExternalController => Some(ControllerMode::ExternalController), + MoveControllerMode::CartesianImpedance => Some(ControllerMode::CartesianImpedance), + }; + let move_command_id = self.execute_move_command( + &controller_mode, + &motion_generator_mode, + maximum_path_deviation, + maximum_goal_deviation, + )?; + + while self.motion_generator_mode.unwrap() != self.current_move_motion_generator_mode + || Some(self.controller_mode) != self.current_move_controller_mode + { + match self + .network + .tcp_receive_response(move_command_id, |x| handle_command_response_move(&x)) + { + Ok(received_message) => { + if received_message { + break; + } + } + Err(FrankaException::CommandException { message }) => { + return Err(FrankaException::ControlException { + log: None, + error: message, + }); + } + _ => { + panic!("this should be an command exception but it is not") + } + } + + let _robot_state = self.update(None, None)?; + } + self.logger.flush(); + Ok(move_command_id) + } + #[allow(unused_must_use)] + fn finish_motion( + &mut self, + motion_id: u32, + motion_command: Option<&MotionGeneratorCommand>, + control_command: Option<&ControllerCommand>, + ) -> FrankaResult<()> { + if !self.motion_generator_running() && !self.controller_running() { + self.current_move_motion_generator_mode = MotionGeneratorMode::Idle; + self.current_move_controller_mode = Some(ControllerMode::Other); + return Ok(()); + } + if motion_command.is_none() { + return Err(FrankaException::ControlException { + log: None, + error: String::from("libfranka-rs robot: No motion generator command given!"), + }); + } + let mut motion_finished_command = *motion_command.unwrap(); + motion_finished_command.motion_generation_finished = true; + let mut robot_state = None; + while self.motion_generator_running() || self.controller_running() { + 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) { + Ok(_) => {} + Err(FrankaException::CommandException { message }) => { + return Err(create_control_exception( + message, + &response.status, + &robot_state.last_motion_errors, + self.logger.flush(), + )); + } + _ => { + panic!("this should be an command exception but it is not") + } + } + self.current_move_motion_generator_mode = MotionGeneratorMode::Idle; + self.current_move_controller_mode = Some(ControllerMode::Other); + Ok(()) + } + + // TODO exception handling + fn cancel_motion(&mut self, motion_id: u32) { + self.execute_stop_command() + .expect("error while canceling motion"); //todo handle Network exception. But it is not that important since we already have an error anyways + let mut _robot_state = self.receive_robot_state(); + while self.motion_generator_running() || self.controller_running() { + _robot_state = self.receive_robot_state(); + } + + // comment from libfranka devs: + // Ignore Move response. + // TODO (FWA): It is not guaranteed that the Move response won't come later + + self.network + .tcp_receive_response(motion_id, |_x: MoveResponse| 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); + } + + fn update( + &mut self, + motion_command: Option<&MotionGeneratorCommand>, + control_command: Option<&ControllerCommand>, + ) -> FrankaResult { + let robot_command = self.send_robot_command(motion_command, control_command)?; + let state = FR3State::from(self.receive_robot_state()?); + if let Some(command) = robot_command { + // self.logger.log(&state, &command); TODO(care about logger) + } + Ok(state) + } + + fn realtime_config(&self) -> RealtimeConfig { + self.realtime_config + } + + fn throw_on_motion_error( + &mut self, + robot_state: &Self::State2, + motion_id: u32, + ) -> FrankaResult<()> { + if robot_state.robot_mode != RobotMode::Move + || 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); + return match result { + Err(error) => Err(create_control_exception( + error.to_string(), + &response.status, + &robot_state.last_motion_errors, + self.logger.flush(), + )), + Ok(_) => panic!("Unexpected reply to a Move command"), + }; + } + Ok(()) } } +// impl MotionGeneratorTrait for RobotImpl { +// fn get_motion_generator_mode() -> MoveMotionGeneratorMode { +// MoveMotionGeneratorMode::JointVelocity +// } +// } + fn create_control_exception( message: String, move_status: &MoveStatus, diff --git a/src/robot/robot_state.rs b/src/robot/robot_state.rs index 2f09a40..25f2f46 100644 --- a/src/robot/robot_state.rs +++ b/src/robot/robot_state.rs @@ -7,6 +7,12 @@ use std::time::Duration; use crate::robot::errors::{FrankaErrorKind, FrankaErrors}; use crate::robot::types::{RobotMode, PandaStateIntern}; use nalgebra::{Matrix3, Vector3}; +use crate::robot::FR3; + +pub trait RobotState { + fn get_time(&self) -> Duration; + fn get_tau_J_d(&self) -> [f64;7]; +} /// Describes the robot state. #[derive(Debug, Clone, Default)] @@ -254,6 +260,252 @@ pub struct PandaState { /// instead pub time: Duration, } + +#[derive(Debug, Clone, Default)] +#[allow(non_snake_case)] +pub struct FR3State { + /// ![^{O}T_{EE}](https://latex.codecogs.com/png.latex?^{O}T_{EE}) + /// + /// Measured end effector pose in base frame. + /// Pose is represented as a 4x4 matrix in column-major format. + pub O_T_EE: [f64; 16], + /// ![{^OT_{EE}}_{d}](http://latex.codecogs.com/png.latex?{^OT_{EE}}_{d}) + /// + /// Last desired end effector pose of motion generation in base frame. + /// Pose is represented as a 4x4 matrix in column-major format. + pub O_T_EE_d: [f64; 16], + /// ![^{F}T_{EE}](http://latex.codecogs.com/png.latex?^{F}T_{EE}) + /// + /// End effector frame pose in flange frame. + /// Pose is represented as a 4x4 matrix in column-major format. + /// # See + /// * [`F_T_NE`](`Self::F_T_NE`) + /// * [`NE_T_EE`](`Self::NE_T_EE`) + /// * [`Robot`](`crate::Robot`) for an explanation of the NE and EE frames. + pub F_T_EE: [f64; 16], + /// ![^{F}T_{NE}](https://latex.codecogs.com/png.latex?^{F}T_{NE}) + /// + /// Nominal end effector frame pose in flange frame. + /// Pose is represented as a 4x4 matrix in column-major format. + /// # See + /// * [`F_T_NE`](`Self::F_T_NE`) + /// * [`NE_T_EE`](`Self::NE_T_EE`) + /// * [`Robot`](`crate::Robot`) for an explanation of the NE and EE frames. + pub F_T_NE: [f64; 16], + /// ![^{NE}T_{EE}](https://latex.codecogs.com/png.latex?^{NE}T_{EE}) + /// + /// End effector frame pose in nominal end effector frame. + /// Pose is represented as a 4x4 matrix in column-major format. + /// # See + /// * [`F_T_NE`](`Self::F_T_NE`) + /// * [`NE_T_EE`](`Self::NE_T_EE`) + /// * [`Robot`](`crate::Robot`) for an explanation of the NE and EE frames. + pub NE_T_EE: [f64; 16], + /// ![^{EE}T_{K}](https://latex.codecogs.com/png.latex?^{EE}T_{K}) + /// + /// Stiffness frame pose in end effector frame. + /// Pose is represented as a 4x4 matrix in column-major format. + /// + /// See also [K-Frame](`crate::Robot#stiffness-frame-k`) + pub EE_T_K: [f64; 16], + /// ![m_{EE}](https://latex.codecogs.com/png.latex?m_{EE}) + /// + /// Configured mass of the end effector. + pub m_ee: f64, + /// ![I_{EE}](https://latex.codecogs.com/png.latex?I_{EE}) + /// + /// Configured rotational inertia matrix of the end effector load with respect to center of mass. + pub I_ee: [f64; 9], + /// ![^{F}x_{C_{EE}}](https://latex.codecogs.com/png.latex?^{F}x_{C_{EE}}) + /// + /// Configured center of mass of the end effector load with respect to flange frame. + pub F_x_Cee: [f64; 3], + /// ![m_{load}](https://latex.codecogs.com/png.latex?m_{load}) + /// + /// Configured mass of the external load. + pub m_load: f64, + /// ![I_{load}](https://latex.codecogs.com/png.latex?I_{load}) + /// + /// Configured rotational inertia matrix of the external load with respect to center of mass. + pub I_load: [f64; 9], + /// ![^{F}x_{C_{load}}](https://latex.codecogs.com/png.latex?^{F}x_{C_{load}}) + /// + /// Configured center of mass of the external load with respect to flange frame. + pub F_x_Cload: [f64; 3], + /// ![m_{total}](https://latex.codecogs.com/png.latex?m_{total}) + /// + /// Sum of the mass of the end effector and the external load. + pub m_total: f64, + /// ![I_{total}](https://latex.codecogs.com/png.latex?I_{total}) + /// + /// Combined rotational inertia matrix of the end effector load and the external load with respect + /// to the center of mass. + pub I_total: [f64; 9], + /// ![^{F}x_{C_{total}}](https://latex.codecogs.com/png.latex?^{F}x_{C_{total}}) + /// + /// Combined center of mass of the end effector load and the external load with respect to flange + /// frame. + pub F_x_Ctotal: [f64; 3], + /// Elbow configuration. + /// + /// The values of the array are: + /// - \[0\] Position of the 3rd joint in \[rad\]. + /// - \[1\] Sign of the 4th joint. Can be +1 or -1. + pub elbow: [f64; 2], + /// Desired elbow configuration. + /// + /// The values of the array are: + /// - \[0\] Position of the 3rd joint in \[rad\]. + /// - \[1\] Sign of the 4th joint. Can be +1 or -1. + pub elbow_d: [f64; 2], + /// Commanded elbow configuration. + /// + /// The values of the array are: + /// - \[0\] Position of the 3rd joint in \[rad\]. + /// - \[1\] Sign of the 4th joint. Can be +1 or -1. + pub elbow_c: [f64; 2], + /// Commanded elbow velocity. + /// + /// The values of the array are: + /// - \[0\] Velocity of the 3rd joint in \[rad/s\]. + /// - \[1\] Sign of the 4th joint. Can be +1 or -1. + pub delbow_c: [f64; 2], + /// Commanded elbow acceleration. + /// + /// The values of the array are: + /// - \[0\] Acceleration of the 3rd joint in \[rad/s^2\]. + /// - \[1\] Sign of the 4th joint. Can be +1 or -1. + pub ddelbow_c: [f64; 2], + /// ![\tau_{J}](https://latex.codecogs.com/png.latex?\tau_{J}) + /// + /// Measured link-side joint torque sensor signals. Unit: \[Nm\] + pub tau_J: [f64; 7], + /// ![{\tau_J}_d](https://latex.codecogs.com/png.latex?{\tau_J}_d) + /// + /// Desired link-side joint torque sensor signals without gravity. Unit: \[Nm\] + pub tau_J_d: [f64; 7], + /// ![\dot{\tau_{J}}](https://latex.codecogs.com/png.latex?\dot{\tau_{J}}) + /// + /// Derivative of measured link-side joint torque sensor signals. Unit: [Nm/s] + pub dtau_J: [f64; 7], + /// ![q](https://latex.codecogs.com/png.latex?q) + /// + /// Measured joint position. Unit: \[rad\] + pub q: [f64; 7], + /// ![q_d](https://latex.codecogs.com/png.latex?q_d) + /// + /// Desired joint position. Unit: \[rad\] + pub q_d: [f64; 7], + /// ![\dot{q}](https://latex.codecogs.com/png.latex?\dot{q}) + /// + /// Measured joint velocity. Unit: \[rad/s\] + pub dq: [f64; 7], + /// ![\dot{q}_d](https://latex.codecogs.com/png.latex?\dot{q}_d) + /// + /// Desired joint velocity. Unit: \[rad/s\] + pub dq_d: [f64; 7], + /// ![\ddot{q}_d](https://latex.codecogs.com/png.latex?\ddot{q}_d) + /// + /// Desired joint acceleration. Unit: \[rad/s^2\] + pub ddq_d: [f64; 7], + /// Indicates which contact level is activated in which joint. After contact disappears, value + /// turns to zero. + /// + /// See [`Robot::set_collision_behavior`](`crate::Robot::set_collision_behavior`) + /// for setting sensitivity values. + pub joint_contact: [f64; 7], + /// Indicates which contact level is activated in which Cartesian dimension (x,y,z,R,P,Y). + /// After contact disappears, the value turns to zero. + /// + /// See [`Robot::set_collision_behavior`](`crate::Robot::set_collision_behavior`) + /// for setting sensitivity values. + pub cartesian_contact: [f64; 6], + /// Indicates which contact level is activated in which joint. After contact disappears, the value + /// stays the same until a reset command is sent. + /// + /// See [`Robot::set_collision_behavior`](`crate::Robot::set_collision_behavior`) + /// for setting sensitivity values. + /// + /// See [`Robot::automatic_error_recovery`](`crate::Robot::automatic_error_recovery`) + /// for performing a reset after a collision. + pub joint_collision: [f64; 7], + /// Indicates which contact level is activated in which Cartesian dimension (x,y,z,R,P,Y). + /// After contact disappears, the value stays the same until a reset command is sent. + /// + /// See [`Robot::set_collision_behavior`](`crate::Robot::set_collision_behavior`) + /// for setting sensitivity values. + /// + /// See [`Robot::automatic_error_recovery`](`crate::Robot::automatic_error_recovery`) + /// for performing a reset after a collision. + pub cartesian_collision: [f64; 6], + /// ![\hat{\tau}_{\text{ext}}](https://latex.codecogs.com/png.latex?\hat{\tau}_{\text{ext}}) + /// + /// External torque, filtered. Unit: \[Nm\] + pub tau_ext_hat_filtered: [f64; 7], + /// ![^OF_{K,\text{ext}}](https://latex.codecogs.com/png.latex?^OF_{K,\text{ext}}) + /// + /// Estimated external wrench (force, torque) acting on stiffness frame, expressed + /// relative to the base frame. See also @ref k-frame "K frame". + /// Unit: \[N,N,N,Nm,Nm,Nm\]. + pub O_F_ext_hat_K: [f64; 6], + /// ![^{K}F_{K,\text{ext}}](https://latex.codecogs.com/png.latex?^{K}F_{K,\text{ext}}) + /// + /// Estimated external wrench (force, torque) acting on stiffness frame, + /// expressed relative to the stiffness frame. See also @ref k-frame "K frame". + /// Unit: \[N,N,N,Nm,Nm,Nm\]. + pub K_F_ext_hat_K: [f64; 6], + /// ![{^OdP_{EE}}_{d}](https://latex.codecogs.com/png.latex?{^OdP_{EE}}_{d}) + /// + /// Desired end effector twist in base frame. + /// Unit: [m/s,m/s,m/s,rad/s,rad/s,rad/s] + pub O_dP_EE_d: [f64; 6], + /// ![{^OddP_O}](https://latex.codecogs.com/png.latex?{^OddP_O) + /// + /// Linear component of the acceleration of the robot's base, expressed in frame parallel to the + /// base frame, i.e. the base's translational acceleration. If the base is resting + /// this shows the direction of the gravity vector. It is hardcoded for now to `{0, 0, -9.81}`. + pub O_ddP_O: [f64; 3], + /// ![{^OT_{EE}}_{c}](https://latex.codecogs.com/png.latex?{^OT_{EE}}_{c}) + /// + /// Last commanded end effector pose of motion generation in base frame. + /// Pose is represented as a 4x4 matrix in column-major format. + pub O_T_EE_c: [f64; 16], + /// ![{^OdP_{EE}}_{c}](https://latex.codecogs.com/png.latex?{^OdP_{EE}}_{c}) + /// + /// Last commanded end effector twist in base frame. + /// Unit: [m/s,m/s,m/s,rad/s,rad/s,rad/s] + pub O_dP_EE_c: [f64; 6], + ///![{^OddP_{EE}}_{c}](https://latex.codecogs.com/png.latex?{^OddP_{EE}}_{c}) + /// + /// Last commanded end effector acceleration in base frame. + /// Unit: [m/s^2,m/s^2,m/s^2,rad/s^2,rad/s^2,rad/s^2] + pub O_ddP_EE_c: [f64; 6], + /// ![\theta](https://latex.codecogs.com/png.latex?\theta) + /// + /// Motor position. Unit: \[rad\] + pub theta: [f64; 7], + /// ![\dot{\theta}](https://latex.codecogs.com/png.latex?\dot{\theta}) + /// + /// Motor velocity. Unit: \[rad/s\] + pub dtheta: [f64; 7], + /// Current error state. + pub current_errors: FrankaErrors, + /// Contains the errors that aborted the previous motion + pub last_motion_errors: FrankaErrors, + /// Percentage of the last 100 control commands that were successfully received by the robot. + /// + /// Shows a value of zero if no control or motion generator loop is currently running. + /// + /// Range \[0,1\] + pub control_command_success_rate: f64, + /// Current robot mode. + pub robot_mode: RobotMode, + /// Strictly monotonically increasing timestamp since robot start. + /// + /// Inside of control loops "time_step" parameter of Robot::control can be used + /// instead + pub time: Duration, +} impl From for PandaState { #[allow(non_snake_case)] fn from(robot_state: PandaStateIntern) -> Self { @@ -371,6 +623,123 @@ impl From for PandaState { } } +impl From for FR3State { + #[allow(non_snake_case)] + 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; + let NE_T_EE = robot_state.NE_T_EE; + let F_T_EE = robot_state.F_T_EE; + let EE_T_K = robot_state.EE_T_K; + let m_ee = robot_state.m_ee; + let F_x_Cee = robot_state.F_x_Cee; + let I_ee = robot_state.I_ee; + let m_load = robot_state.m_load; + let F_x_Cload = robot_state.F_x_Cload; + let I_load = robot_state.I_load; + let m_total = robot_state.m_ee + robot_state.m_load; + let F_x_Ctotal = combine_center_of_mass( + robot_state.m_ee, + robot_state.F_x_Cee, + robot_state.m_load, + robot_state.F_x_Cload, + ); + let I_total = combine_inertia_tensor( + robot_state.m_ee, + robot_state.F_x_Cee, + robot_state.I_ee, + robot_state.m_load, + robot_state.F_x_Cload, + robot_state.I_load, + m_total, + F_x_Ctotal, + ); + let elbow = robot_state.elbow; + let elbow_d = robot_state.elbow_d; + let elbow_c = robot_state.elbow_c; + let delbow_c = robot_state.delbow_c; + let ddelbow_c = robot_state.ddelbow_c; + let tau_J = robot_state.tau_J; + let tau_J_d = robot_state.tau_J_d; + let dtau_J = robot_state.dtau_J; + let q = robot_state.q; + let dq = robot_state.dq; + let q_d = robot_state.q_d; + let dq_d = robot_state.dq_d; + let ddq_d = robot_state.ddq_d; + let joint_contact = robot_state.joint_contact; + let cartesian_contact = robot_state.cartesian_contact; + let joint_collision = robot_state.joint_collision; + let cartesian_collision = robot_state.cartesian_collision; + let tau_ext_hat_filtered = robot_state.tau_ext_hat_filtered; + let O_F_ext_hat_K = robot_state.O_F_ext_hat_K; + let K_F_ext_hat_K = robot_state.K_F_ext_hat_K; + let O_dP_EE_d = robot_state.O_dP_EE_d; + let O_ddP_O = robot_state.O_ddP_O; + let O_T_EE_c = robot_state.O_T_EE_c; + let O_dP_EE_c = robot_state.O_dP_EE_c; + let O_ddP_EE_c = robot_state.O_ddP_EE_c; + let theta = robot_state.theta; + let dtheta = robot_state.dtheta; + let control_command_success_rate = robot_state.control_command_success_rate; + let time = Duration::from_millis(robot_state.message_id); + let robot_mode = robot_state.robot_mode; + let current_errors = FrankaErrors::new(robot_state.errors, FrankaErrorKind::Error); + let last_motion_errors = + FrankaErrors::new(robot_state.errors, FrankaErrorKind::ReflexReason); + FR3State { + O_T_EE, + O_T_EE_d, + F_T_EE, + F_T_NE, + NE_T_EE, + EE_T_K, + m_ee, + I_ee, + F_x_Cee, + m_load, + I_load, + F_x_Cload, + m_total, + I_total, + F_x_Ctotal, + elbow, + elbow_d, + elbow_c, + delbow_c, + ddelbow_c, + tau_J, + tau_J_d, + dtau_J, + q, + q_d, + dq, + dq_d, + ddq_d, + joint_contact, + cartesian_contact, + joint_collision, + cartesian_collision, + tau_ext_hat_filtered, + O_F_ext_hat_K, + K_F_ext_hat_K, + O_dP_EE_d, + O_ddP_O, + O_T_EE_c, + O_dP_EE_c, + O_ddP_EE_c, + theta, + dtheta, + current_errors, + last_motion_errors, + control_command_success_rate, + robot_mode, + time, + } + } +} + #[allow(non_snake_case, clippy::too_many_arguments)] fn combine_inertia_tensor( m_ee: f64, @@ -438,6 +807,27 @@ fn skew_symmetric_matrix_from_vector(vector: &Vector3) -> Matrix3 { ) } +impl RobotState for PandaState { + fn get_time(&self) -> Duration { + self.time + } + + fn get_tau_J_d(&self) -> [f64; 7] { + self.tau_J_d + } +} + +impl RobotState for FR3State { + fn get_time(&self) -> Duration { + self.time + } + + + fn get_tau_J_d(&self) -> [f64; 7] { + self.tau_J_d + } +} + #[cfg(test)] mod tests { use crate::robot::robot_state::{combine_center_of_mass, combine_inertia_tensor}; diff --git a/src/robot/types.rs b/src/robot/types.rs index efd7453..ff2b2e3 100644 --- a/src/robot/types.rs +++ b/src/robot/types.rs @@ -280,3 +280,5 @@ pub struct RobotCommand { pub motion: MotionGeneratorCommandPacked, pub control: ControllerCommandPacked, } + +pub trait ConvertMotion {} diff --git a/src/utils.rs b/src/utils.rs index 29488bd..77e79bd 100644 --- a/src/utils.rs +++ b/src/utils.rs @@ -2,10 +2,11 @@ // 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::control_types::{ConvertMotion, JointPositions}; use crate::robot::robot_state::PandaState; use nalgebra::{Isometry3, Matrix4, MatrixMN, MatrixN, Rotation3, Vector3, VectorN, U6, U7}; use std::time::Duration; +use crate::Finishable; /// converts a 4x4 column-major homogenous matrix to an Isometry pub fn array_to_isometry(array: &[f64; 16]) -> Isometry3 { @@ -192,7 +193,7 @@ impl MotionGenerator { #[cfg(test)] mod test { - use crate::{array_to_isometry, Finishable, MotionGenerator, PandaState}; + use crate::{array_to_isometry, ConvertMotion, MotionGenerator, PandaState}; use nalgebra::Rotation3; use std::time::Duration; From 9008233f2b102551f2c89399d7046325861e3734 Mon Sep 17 00:00:00 2001 From: Marco Boneberger Date: Fri, 11 Nov 2022 13:01:17 +0100 Subject: [PATCH 03/63] make network generic over robot data --- src/gripper.rs | 42 ++++----- src/gripper/types.rs | 44 +++++++--- src/model/library_downloader.rs | 44 ++++++++-- src/network.rs | 144 ++++++++++++++++++++++--------- src/robot.rs | 88 +++++++++---------- src/robot/robot_impl.rs | 45 +++++----- src/robot/service_types.rs | 145 ++++++++++++++++++++++++++------ src/utils.rs | 2 +- 8 files changed, 379 insertions(+), 175 deletions(-) diff --git a/src/gripper.rs b/src/gripper.rs index b19ac55..2bcef66 100644 --- a/src/gripper.rs +++ b/src/gripper.rs @@ -8,12 +8,12 @@ use std::mem::size_of; use crate::exception::{create_command_exception, FrankaException, FrankaResult}; use crate::gripper::gripper_state::GripperState; use crate::gripper::types::{ - Command, ConnectRequest, ConnectRequestWithHeader, ConnectResponse, GraspRequest, + GripperCommandEnum, ConnectRequest, ConnectRequestWithHeader, ConnectResponse, GraspRequest, GraspRequestWithHeader, GraspResponse, GripperStateIntern, HomingRequestWithHeader, HomingResponse, MoveRequest, MoveRequestWithHeader, MoveResponse, Status, StopRequestWithHeader, StopResponse, COMMAND_PORT, VERSION, }; -use crate::network::{Network, NetworkType}; +use crate::network::{GripperData, Network, NetworkType}; pub mod gripper_state; pub(crate) mod types; @@ -21,7 +21,7 @@ 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, } @@ -48,7 +48,7 @@ impl Gripper { let connect_command = ConnectRequestWithHeader { header: self .network - .create_header_for_gripper(Command::Connect, size_of::()), + .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,7 +78,7 @@ 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); @@ -98,7 +98,7 @@ impl Gripper { pub fn homing(&mut self) -> FrankaResult { let command: HomingRequestWithHeader = self .network - .create_header_for_gripper(Command::Homing, size_of::()); + .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) @@ -113,7 +113,7 @@ impl Gripper { pub fn stop(&mut self) -> FrankaResult { let command: StopRequestWithHeader = 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) @@ -149,7 +149,7 @@ impl Gripper { let command = GraspRequestWithHeader { header: self .network - .create_header_for_gripper(Command::Grasp, size_of::()), + .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); @@ -194,7 +194,7 @@ fn handle_response_status(status: &Status) -> FrankaResult { #[cfg(test)] mod tests { use crate::gripper::types::{ - Command, CommandHeader, ConnectRequestWithHeader, ConnectResponse, GraspRequest, + GripperCommandEnum, GripperCommandHeader, ConnectRequestWithHeader, ConnectResponse, GraspRequest, GraspRequestWithHeader, GraspResponse, GripperStateIntern, HomingRequestWithHeader, HomingResponse, MoveRequest, MoveRequestWithHeader, MoveResponse, Status, StopRequestWithHeader, StopResponse, COMMAND_PORT, VERSION, @@ -359,8 +359,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 +384,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, ), @@ -416,7 +416,7 @@ mod tests { let req: MoveRequestWithHeader = deserialize(&bytes).unwrap(); counter += 1; let mut response = MoveResponse { - header: CommandHeader::new(Command::Move, req.header.command_id, 0), + header: GripperCommandHeader::new(GripperCommandEnum::Move, req.header.command_id, 0), status: Status::Success, }; response.header.size = serialized_size(&response).unwrap() as u32; @@ -447,13 +447,13 @@ mod tests { .returning(move |bytes: &mut Vec| -> Vec { let req: StopRequestWithHeader = 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), + header: GripperCommandHeader::new(GripperCommandEnum::Stop, req.command_id, 0), status: Status::Success, }; response.header.size = serialized_size(&response).unwrap() as u32; @@ -481,13 +481,13 @@ mod tests { .returning(move |bytes: &mut Vec| -> Vec { let req: HomingRequestWithHeader = 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), + header: GripperCommandHeader::new(GripperCommandEnum::Homing, req.command_id, 0), status: Status::Success, }; response.header.size = serialized_size(&response).unwrap() as u32; @@ -518,8 +518,8 @@ mod tests { -> GraspRequestWithHeader { counter += 1; GraspRequestWithHeader { - header: CommandHeader::new( - Command::Grasp, + header: GripperCommandHeader::new( + GripperCommandEnum::Grasp, counter, size_of::() as u32, ), @@ -549,7 +549,7 @@ mod tests { let req: MoveRequestWithHeader = deserialize(&bytes).unwrap(); counter += 1; let mut response = GraspResponse { - header: CommandHeader::new(Command::Grasp, req.header.command_id, 0), + header: GripperCommandHeader::new(GripperCommandEnum::Grasp, req.header.command_id, 0), status: Status::Success, }; response.header.size = serialized_size(&response).unwrap() as u32; diff --git a/src/gripper/types.rs b/src/gripper/types.rs index fcbf839..39e6ae9 100644 --- a/src/gripper/types.rs +++ b/src/gripper/types.rs @@ -9,13 +9,14 @@ use serde_repr::{Deserialize_repr, Serialize_repr}; use crate::network::MessageCommand; use std::time::Duration; +use serde::de::DeserializeOwned; pub static 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, @@ -48,17 +49,32 @@ impl GripperStateIntern { } } +pub trait CommandHeader : 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, @@ -127,28 +143,28 @@ 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; +pub type HomingRequestWithHeader = GripperCommandHeader; +pub type StopRequestWithHeader = GripperCommandHeader; #[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,14 +190,14 @@ 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 header: GripperCommandHeader, pub status: Status, } diff --git a/src/model/library_downloader.rs b/src/model/library_downloader.rs index 5a7b99d..70f9c38 100644 --- a/src/model/library_downloader.rs +++ b/src/model/library_downloader.rs @@ -1,10 +1,10 @@ // Copyright (c) 2021 Marco Boneberger // Licensed under the EUPL-1.2-or-later use crate::exception::FrankaException::ModelException; -use crate::network::Network; +use crate::network::{FR3Data, Network, PandaData}; use crate::robot::service_types::{ LoadModelLibraryArchitecture, LoadModelLibraryRequest, LoadModelLibraryRequestWithHeader, - LoadModelLibraryResponse, LoadModelLibrarySystem, RobotCommandEnum, + LoadModelLibraryResponse, LoadModelLibrarySystem, PandaCommandEnum, }; use crate::FrankaResult; use std::fmt; @@ -18,11 +18,11 @@ use std::path::Path; pub struct LibraryDownloader {} impl LibraryDownloader { - pub fn download(network: &mut Network, download_path: &Path) -> FrankaResult<()> { + 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, + header: network.create_header( + PandaCommandEnum::LoadModelLibrary, size_of::(), ), request: LoadModelLibraryRequest { @@ -50,6 +50,40 @@ impl LibraryDownloader { }) } } + + // TODO unify + pub fn download2(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_panda( + PandaCommandEnum::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(()) + } else { + Err(ModelException { + message: + "Your platform is not yet supported for Downloading models. Please use Linux on\ + x86_64 for now" + .to_string(), + }) + } + } } #[derive(Debug)] diff --git a/src/network.rs b/src/network.rs index fd26f95..f8b1c03 100644 --- a/src/network.rs +++ b/src/network.rs @@ -9,6 +9,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,25 +27,84 @@ 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::{gripper, GripperState, PandaState}; +use crate::gripper::types::{CommandHeader, GripperCommandEnum, GripperCommandHeader}; +use crate::robot::robot_state::FR3State; +use crate::robot::service_types::{FR3CommandEnum, FR3CommandHeader, LoadModelLibraryResponse, LoadModelLibraryStatus, PandaCommandEnum, PandaCommandHeader}; const CLIENT: Token = Token(1); pub enum NetworkType { - Robot, + Panda, + FR3, Gripper, } +pub trait RobotData { + type CommandHeader: CommandHeader; + type CommandEnum; + type State; + + fn create_header(command_id: &mut u32, + command: Self::CommandEnum, + size: usize, + ) -> Self::CommandHeader; + +} + +pub struct PandaData { + +} + +pub struct FR3Data { + +} + +pub struct GripperData{ + +} + +impl RobotData for PandaData { + type CommandHeader = PandaCommandHeader; + type CommandEnum = PandaCommandEnum; + type State = PandaState; + + 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 + } +} + +impl RobotData for FR3Data { + type CommandHeader = FR3CommandHeader; + type CommandEnum = FR3CommandEnum; + type State = FR3State; + + 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 + } +} + +impl RobotData for GripperData { + type CommandHeader = GripperCommandHeader; + type CommandEnum = GripperCommandEnum; + type State = GripperState; + + 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 + } +} + pub trait MessageCommand { fn get_command_message_id(&self) -> u32; } -pub struct Network { - network_type: NetworkType, +pub struct Network { tcp_socket: TcpStream, udp_socket: UdpSocket, udp_server_address: SocketAddr, @@ -60,14 +120,15 @@ pub struct Network { events: Events, poll_read_udp: Poll, events_udp: Events, + data : PhantomData, } -impl Network { +impl Network { pub fn new( network_type: NetworkType, franka_address: &str, franka_port: u16, - ) -> Result> { + ) -> 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 +162,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,26 +177,45 @@ impl Network { events, poll_read_udp, events_udp, + data:PhantomData }) } - pub fn create_header_for_gripper( + // pub fn create_header_for_gripper( + // &mut self, + // command: gripper::types::GripperCommandEnum, + // size: usize, + // ) -> GripperCommandHeader { + // let header = gripper::types::GripperCommandHeader::new(command, self.command_id, size as u32); + // self.command_id += 1; + // header + // } + 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 create_header_for_fr3( + // &mut self, + // command: FR3CommandEnum, + // size: usize, + // ) -> FR3CommandHeader { + // let header = FR3CommandHeader::new(command, self.command_id, size as u32); + // self.command_id += 1; + // header + // } pub fn tcp_send_request( &mut self, @@ -207,7 +286,7 @@ impl Network { /// /// # Error /// * [`FrankaException`](`crate::exception::FrankaException`) - if the handler returns an exception - pub fn tcp_receive_response( + pub fn tcp_receive_response( &mut self, command_id: u32, handler: F, @@ -334,27 +413,15 @@ 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; - } - }; + 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 { @@ -400,6 +467,7 @@ fn deserialize(encoded: &[u8]) -> T { #[cfg(test)] mod tests { use crate::network::{deserialize, serialize}; + use crate::robot::service_types::PandaCommandHeader; use crate::robot::types::PandaStateIntern; #[test] diff --git a/src/robot.rs b/src/robot.rs index 6515cf6..3049944 100644 --- a/src/robot.rs +++ b/src/robot.rs @@ -9,7 +9,7 @@ use std::fmt::Debug; use crate::exception::{create_command_exception, FrankaException, FrankaResult}; use crate::model::Model; -use crate::network::{Network, NetworkType}; +use crate::network::{FR3Data, Network, NetworkType, PandaData, RobotData}; use crate::robot::control_loop::ControlLoop; use crate::robot::control_types::{ CartesianPose, CartesianVelocities, ControllerMode, ConvertMotion, JointPositions, @@ -22,7 +22,7 @@ use crate::robot::robot_impl::{RobotImplPanda, RobotImplementation, RobotImplFR3 use crate::robot::service_types::{ AutomaticErrorRecoveryRequestWithHeader, AutomaticErrorRecoveryResponse, AutomaticErrorRecoveryStatus, GetCartesianLimitRequest, GetCartesianLimitRequestWithHeader, - GetCartesianLimitResponse, GetterSetterStatus, RobotCommandEnum, SetCartesianImpedanceRequest, + GetCartesianLimitResponse, GetterSetterStatus, PandaCommandEnum, SetCartesianImpedanceRequest, SetCartesianImpedanceRequestWithHeader, SetCartesianImpedanceResponse, SetCollisionBehaviorRequest, SetCollisionBehaviorRequestWithHeader, SetCollisionBehaviorResponse, SetEeToKRequest, SetEeToKRequestWithHeader, SetEeToKResponse, @@ -55,10 +55,11 @@ pub(crate) mod types; pub mod virtual_wall_cuboid; pub trait Robot where CartesianPose: ConvertMotion<::State1>{ + type Data:RobotData; type State1:RobotState; type Rob:RobotImplementation; fn get_rob(&mut self) -> &mut Self::Rob; - fn get_net(&mut self) -> &mut Network; + fn get_net(&mut self) -> &mut Network; fn read bool>(&mut self, mut read_callback: F, ) -> FrankaResult<()> { @@ -88,8 +89,8 @@ pub trait Robot where CartesianPose: ConvertMotion<::State1>{ upper_force_thresholds_nominal: [f64; 6], ) -> FrankaResult<()> { let command = SetCollisionBehaviorRequestWithHeader { - header: self.get_net().create_header_for_robot( - RobotCommandEnum::SetCollisionBehavior, + header: self.get_net().create_header_for_panda( + PandaCommandEnum::SetCollisionBehavior, size_of::(), ), request: SetCollisionBehaviorRequest::new( @@ -113,8 +114,8 @@ pub trait Robot where CartesianPose: ConvertMotion<::State1>{ #[allow(non_snake_case)] fn set_joint_impedance(&mut self, K_theta: [f64; 7]) -> FrankaResult<()> { let command = SetJointImpedanceRequestWithHeader { - header: self.get_net().create_header_for_robot( - RobotCommandEnum::SetJointImpedance, + header: self.get_net().create_header_for_panda( + PandaCommandEnum::SetJointImpedance, size_of::(), ), request: SetJointImpedanceRequest::new(K_theta), @@ -129,8 +130,8 @@ pub trait Robot where CartesianPose: ConvertMotion<::State1>{ #[allow(non_snake_case)] fn set_cartesian_impedance(&mut self, K_x: [f64; 6]) -> FrankaResult<()> { let command = SetCartesianImpedanceRequestWithHeader { - header: self.get_net().create_header_for_robot( - RobotCommandEnum::SetCartesianImpedance, + header: self.get_net().create_header_for_panda( + PandaCommandEnum::SetCartesianImpedance, size_of::(), ), request: SetCartesianImpedanceRequest::new(K_x), @@ -244,9 +245,10 @@ pub struct Panda { } impl Robot for Panda { + type Data = PandaData; type State1 = PandaState; type Rob = RobotImplPanda; - fn get_net(&mut self) -> &mut Network { + fn get_net(&mut self) -> &mut Network{ &mut self.robimpl.network } fn get_rob(&mut self) ->&mut Self::Rob { @@ -259,9 +261,10 @@ pub struct FR3 { } impl Robot for FR3 { + type Data = FR3Data; type State1 = FR3State; type Rob = RobotImplFR3; - fn get_net(&mut self) -> &mut Network { + fn get_net(&mut self) -> &mut Network { &mut self.robimpl.network } fn get_rob(&mut self) ->&mut Self::Rob { @@ -277,8 +280,7 @@ impl FR3{ ) -> 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, + let network = Network::new(NetworkType::FR3, franka_address, service_types::COMMAND_PORT, ) @@ -321,8 +323,7 @@ impl Panda { ) -> 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, + let network = Network::new(NetworkType::Panda, franka_address, service_types::COMMAND_PORT, ) @@ -510,8 +511,8 @@ impl Panda { /// * [`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, + header: self.robimpl.network.create_header_for_panda( + PandaCommandEnum::SetGuidingMode, size_of::(), ), request: SetGuidingModeRequest::new(guiding_mode, elbow), @@ -537,8 +538,8 @@ impl Panda { #[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, + header: self.robimpl.network.create_header_for_panda( + PandaCommandEnum::SetEeToK, size_of::(), ), request: SetEeToKRequest::new(EE_T_K), @@ -568,8 +569,8 @@ impl Panda { #[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, + header: self.robimpl.network.create_header_for_panda( + PandaCommandEnum::SetNeToEe, size_of::(), ), request: SetNeToEeRequest::new(NE_T_EE), @@ -603,8 +604,8 @@ impl Panda { load_inertia: [f64; 9], ) -> FrankaResult<()> { let command = SetLoadRequestWithHeader { - header: self.robimpl.network.create_header_for_robot( - RobotCommandEnum::SetLoad, + header: self.robimpl.network.create_header_for_panda( + PandaCommandEnum::SetLoad, size_of::(), ), request: SetLoadRequest::new(load_mass, F_x_Cload, load_inertia), @@ -643,8 +644,8 @@ impl Panda { controller_filter_frequency: f64, ) -> FrankaResult<()> { let command = SetFiltersRequestWithHeader { - header: self.robimpl.network.create_header_for_robot( - RobotCommandEnum::SetFilters, + header: self.robimpl.network.create_header_for_panda( + PandaCommandEnum::SetFilters, size_of::(), ), request: SetFiltersRequest::new( @@ -670,8 +671,8 @@ impl Panda { /// * [`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, + let command = self.robimpl.network.create_header_for_panda( + PandaCommandEnum::AutomaticErrorRecovery, size_of::(), ); let command_id: u32 = self.robimpl.network.tcp_send_request(command); @@ -712,8 +713,8 @@ impl Panda { /// * [`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, + header: self.robimpl.network.create_header_for_panda( + PandaCommandEnum::StopMove, size_of::(), ), }; @@ -747,8 +748,8 @@ impl Panda { /// * [`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, + header: self.robimpl.network.create_header_for_panda( + PandaCommandEnum::GetCartesianLimit, size_of::(), ), request: GetCartesianLimitRequest::new(id), @@ -1273,16 +1274,17 @@ mod tests { use crate::robot::service_types::{ ConnectRequestWithHeader, ConnectResponse, ConnectStatus, GetterSetterStatus, MoveControllerMode, MoveDeviation, MoveMotionGeneratorMode, MoveRequest, - MoveRequestWithHeader, MoveResponse, MoveStatus, RobotCommandEnum, RobotCommandHeader, + MoveRequestWithHeader, MoveResponse, MoveStatus, PandaCommandEnum, PandaCommandHeader, SetCollisionBehaviorRequest, SetCollisionBehaviorRequestWithHeader, SetCollisionBehaviorResponse, COMMAND_PORT, VERSION, }; use crate::robot::types::PandaStateIntern; - use crate::{FrankaResult, JointPositions, MotionFinished, RealtimeConfig, Panda, PandaState}; + use crate::{FrankaResult, JointPositions, RealtimeConfig, Panda, PandaState, Finishable}; use bincode::{deserialize, serialize, serialized_size}; use std::iter::FromIterator; use std::mem::size_of; use std::time::{Duration, Instant}; + use crate::robot::Robot; struct Socket), G: Fn(&mut Vec)> { pub send_bytes: F, @@ -1423,8 +1425,8 @@ mod tests { tcp_socket: &mut Socket, ) { let mut response = ConnectResponse { - header: RobotCommandHeader { - command: RobotCommandEnum::Connect, + header: PandaCommandHeader { + command: PandaCommandEnum::Connect, command_id: request.get_command_message_id(), size: 0, }, @@ -1465,8 +1467,8 @@ mod tests { upper_force_thresholds_nominal: [f64; 6]| { counter += 1; SetCollisionBehaviorRequestWithHeader { - header: RobotCommandHeader::new( - RobotCommandEnum::SetCollisionBehavior, + header: PandaCommandHeader::new( + PandaCommandEnum::SetCollisionBehavior, counter, size_of::() as u32, ), @@ -1507,8 +1509,8 @@ mod tests { let req: SetCollisionBehaviorRequestWithHeader = deserialize(&bytes).unwrap(); counter += 1; let mut response = SetCollisionBehaviorResponse { - header: RobotCommandHeader::new( - RobotCommandEnum::SetCollisionBehavior, + header: PandaCommandHeader::new( + PandaCommandEnum::SetCollisionBehavior, req.header.command_id, 0, ), @@ -1539,8 +1541,8 @@ mod tests { #[test] fn fail_start_motion_test() { let requests = Arc::new(vec![MoveRequestWithHeader { - header: RobotCommandHeader::new( - RobotCommandEnum::Move, + header: PandaCommandHeader::new( + PandaCommandEnum::Move, 1, size_of::() as u32, ), @@ -1577,8 +1579,8 @@ mod tests { .for_each(|(x, y)| assert_eq!(x, y)); counter += 1; let mut response = MoveResponse { - header: RobotCommandHeader::new( - RobotCommandEnum::Move, + header: PandaCommandHeader::new( + PandaCommandEnum::Move, req.header.command_id, 0, ), diff --git a/src/robot/robot_impl.rs b/src/robot/robot_impl.rs index f5c4f98..7a930dc 100644 --- a/src/robot/robot_impl.rs +++ b/src/robot/robot_impl.rs @@ -5,19 +5,14 @@ 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::network::Network; +use crate::network::{FR3Data, Network, PandaData, RobotData}; use crate::robot::control_types::RealtimeConfig; use crate::robot::errors::FrankaErrors; use crate::robot::logger::{Logger, Record}; use crate::robot::robot_control::RobotControl; use crate::robot::robot_state::{FR3State, PandaState, RobotState}; -use crate::robot::service_types::{ - ConnectRequest, ConnectRequestWithHeader, ConnectResponse, ConnectStatus, MoveControllerMode, - MoveDeviation, MoveMotionGeneratorMode, MoveRequest, MoveRequestWithHeader, MoveResponse, - MoveStatus, RobotCommandEnum, StopMoveRequestWithHeader, StopMoveResponse, StopMoveStatus, - VERSION, -}; +use crate::robot::service_types::{ConnectRequest, ConnectRequestWithHeader, ConnectResponse, ConnectStatus, MoveControllerMode, MoveDeviation, MoveMotionGeneratorMode, MoveRequest, MoveRequestWithHeader, MoveResponse, MoveStatus, PandaCommandEnum, PandaCommandHeader, StopMoveRequestWithHeader, StopMoveResponse, StopMoveStatus, VERSION}; use crate::robot::types::{ ControllerCommand, ControllerMode, MotionGeneratorCommand, MotionGeneratorMode, RobotCommand, RobotMode, PandaStateIntern, @@ -90,7 +85,7 @@ impl RobotImplementation for RobotImplFR3 { 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)?; + LibraryDownloader::download2(&mut self.network, model_file)?; } let model = Model::new(model_file, None)?; if !persistent && model_already_downloaded { @@ -104,7 +99,7 @@ impl RobotImplementation for RobotImplFR3 { } pub struct RobotImplPanda { - pub network: Network, + pub network: Network, logger: Logger, realtime_config: RealtimeConfig, ri_version: Option, @@ -116,7 +111,7 @@ pub struct RobotImplPanda { } pub struct RobotImplFR3 { - pub network: Network, + pub network: Network, logger: Logger, realtime_config: RealtimeConfig, ri_version: Option, @@ -181,7 +176,7 @@ fn handle_command_response_stop(response: &StopMoveResponse) -> Result<(), Frank impl RobotImplPanda { pub fn new( - network: Network, + network: Network, log_size: usize, realtime_config: RealtimeConfig, ) -> FrankaResult { @@ -209,8 +204,8 @@ impl RobotImplPanda { } fn connect_robot(&mut self) -> Result<(), FrankaException> { let connect_command = ConnectRequestWithHeader { - header: self.network.create_header_for_robot( - RobotCommandEnum::Connect, + header:PandaData::create_header(&mut self.network.command_id, + PandaCommandEnum::Connect, size_of::(), ), request: ConnectRequest::new(self.network.get_udp_port()), @@ -343,8 +338,8 @@ impl RobotImplPanda { maximum_goal_deviation: &MoveDeviation, ) -> FrankaResult { let connect_command = MoveRequestWithHeader { - header: self.network.create_header_for_robot( - RobotCommandEnum::Move, + header: self.network.create_header_for_panda( + PandaCommandEnum::Move, size_of::(), ), request: MoveRequest::new( @@ -361,8 +356,8 @@ impl RobotImplPanda { } fn execute_stop_command(&mut self) -> FrankaResult { let command = StopMoveRequestWithHeader { - header: self.network.create_header_for_robot( - RobotCommandEnum::StopMove, + header: self.network.create_header_for_panda( + PandaCommandEnum::StopMove, size_of::(), ), }; @@ -375,7 +370,7 @@ impl RobotImplPanda { impl RobotImplFR3 { pub fn new( - network: Network, + network: Network, log_size: usize, realtime_config: RealtimeConfig, ) -> FrankaResult { @@ -403,8 +398,8 @@ impl RobotImplFR3 { } fn connect_robot(&mut self) -> Result<(), FrankaException> { let connect_command = ConnectRequestWithHeader { - header: self.network.create_header_for_robot( - RobotCommandEnum::Connect, + header: self.network.create_header_for_panda( + PandaCommandEnum::Connect, size_of::(), ), request: ConnectRequest::new(self.network.get_udp_port()), @@ -518,7 +513,7 @@ impl RobotImplFR3 { 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)?; + LibraryDownloader::download2(&mut self.network, model_file)?; } let model = Model::new(model_file, None)?; if !persistent && model_already_downloaded { @@ -537,8 +532,8 @@ impl RobotImplFR3 { maximum_goal_deviation: &MoveDeviation, ) -> FrankaResult { let connect_command = MoveRequestWithHeader { - header: self.network.create_header_for_robot( - RobotCommandEnum::Move, + header: self.network.create_header_for_panda( + PandaCommandEnum::Move, size_of::(), ), request: MoveRequest::new( @@ -555,8 +550,8 @@ impl RobotImplFR3 { } fn execute_stop_command(&mut self) -> FrankaResult { let command = StopMoveRequestWithHeader { - header: self.network.create_header_for_robot( - RobotCommandEnum::StopMove, + header: self.network.create_header_for_panda( + PandaCommandEnum::StopMove, size_of::(), ), }; diff --git a/src/robot/service_types.rs b/src/robot/service_types.rs index f6e2e43..d731278 100644 --- a/src/robot/service_types.rs +++ b/src/robot/service_types.rs @@ -6,15 +6,56 @@ use std::fmt::Debug; use serde::Deserialize; use serde::Serialize; use serde_repr::{Deserialize_repr, Serialize_repr}; +use crate::gripper::types::CommandHeader; use crate::network::MessageCommand; pub static VERSION: u16 = 5; 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 +72,25 @@ 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 { @@ -115,7 +175,7 @@ impl ConnectRequest { #[derive(Serialize, Deserialize, Debug, Copy, Clone)] #[repr(packed)] pub struct ConnectRequestWithHeader { - pub header: RobotCommandHeader, + pub header: PandaCommandHeader, pub request: ConnectRequest, } @@ -127,7 +187,7 @@ impl MessageCommand for ConnectRequestWithHeader { #[derive(Serialize, Deserialize, Debug)] pub struct ConnectResponse { - pub header: RobotCommandHeader, + pub header: PandaCommandHeader, pub status: ConnectStatus, pub version: u16, } @@ -185,7 +245,7 @@ impl MoveRequest { #[derive(Serialize, Deserialize, Debug, Copy, Clone)] #[repr(packed)] pub struct MoveRequestWithHeader { - pub header: RobotCommandHeader, + pub header: PandaCommandHeader, pub request: MoveRequest, } @@ -197,14 +257,14 @@ impl MessageCommand for MoveRequestWithHeader { #[derive(Serialize, Deserialize, Debug)] pub struct MoveResponse { - pub header: RobotCommandHeader, + pub header: PandaCommandHeader, pub status: MoveStatus, } #[derive(Serialize, Deserialize, Debug, Copy, Clone)] #[repr(packed)] pub struct StopMoveRequestWithHeader { - pub header: RobotCommandHeader, + pub header: PandaCommandHeader, } impl MessageCommand for StopMoveRequestWithHeader { @@ -215,7 +275,7 @@ impl MessageCommand for StopMoveRequestWithHeader { #[derive(Serialize, Deserialize, Debug)] pub struct StopMoveResponse { - pub header: RobotCommandHeader, + pub header: PandaCommandHeader, pub status: StopMoveStatus, } @@ -234,7 +294,7 @@ impl GetCartesianLimitRequest { #[derive(Serialize, Deserialize, Debug, Copy, Clone)] #[repr(packed)] pub struct GetCartesianLimitRequestWithHeader { - pub header: RobotCommandHeader, + pub header: PandaCommandHeader, pub request: GetCartesianLimitRequest, } @@ -246,7 +306,7 @@ impl MessageCommand for GetCartesianLimitRequestWithHeader { #[derive(Serialize, Deserialize, Debug)] pub struct GetCartesianLimitResponse { - pub header: RobotCommandHeader, + pub header: PandaCommandHeader, pub status: GetterSetterStatus, pub object_world_size: [f64; 3], pub object_frame: [f64; 16], @@ -294,7 +354,7 @@ impl SetCollisionBehaviorRequest { #[derive(Serialize, Deserialize, Debug, Copy, Clone)] #[repr(packed)] pub struct SetCollisionBehaviorRequestWithHeader { - pub header: RobotCommandHeader, + pub header: PandaCommandHeader, pub request: SetCollisionBehaviorRequest, } @@ -323,7 +383,7 @@ impl SetJointImpedanceRequest { #[derive(Serialize, Deserialize, Debug, Copy, Clone)] #[repr(packed)] pub struct SetJointImpedanceRequestWithHeader { - pub header: RobotCommandHeader, + pub header: PandaCommandHeader, pub request: SetJointImpedanceRequest, } @@ -352,7 +412,7 @@ impl SetCartesianImpedanceRequest { #[derive(Serialize, Deserialize, Debug, Copy, Clone)] #[repr(packed)] pub struct SetCartesianImpedanceRequestWithHeader { - pub header: RobotCommandHeader, + pub header: PandaCommandHeader, pub request: SetCartesianImpedanceRequest, } @@ -385,7 +445,7 @@ impl SetGuidingModeRequest { #[derive(Serialize, Deserialize, Debug, Copy, Clone)] #[repr(packed)] pub struct SetGuidingModeRequestWithHeader { - pub header: RobotCommandHeader, + pub header: PandaCommandHeader, pub request: SetGuidingModeRequest, } @@ -414,7 +474,7 @@ impl SetEeToKRequest { #[derive(Serialize, Deserialize, Debug, Copy, Clone)] #[repr(packed)] pub struct SetEeToKRequestWithHeader { - pub header: RobotCommandHeader, + pub header: PandaCommandHeader, pub request: SetEeToKRequest, } @@ -443,7 +503,7 @@ impl SetNeToEeRequest { #[derive(Serialize, Deserialize, Debug, Copy, Clone)] #[repr(packed)] pub struct SetNeToEeRequestWithHeader { - pub header: RobotCommandHeader, + pub header: PandaCommandHeader, pub request: SetNeToEeRequest, } @@ -478,7 +538,7 @@ impl SetLoadRequest { #[derive(Serialize, Deserialize, Debug, Copy, Clone)] #[repr(packed)] pub struct SetLoadRequestWithHeader { - pub header: RobotCommandHeader, + pub header: PandaCommandHeader, pub request: SetLoadRequest, } @@ -523,7 +583,7 @@ impl SetFiltersRequest { #[derive(Serialize, Deserialize, Debug, Copy, Clone)] #[repr(packed)] pub struct SetFiltersRequestWithHeader { - pub header: RobotCommandHeader, + pub header: PandaCommandHeader, pub request: SetFiltersRequest, } @@ -537,15 +597,15 @@ pub type SetFiltersResponse = SetterResponse; #[derive(Serialize, Deserialize, Debug)] pub struct SetterResponse { - pub header: RobotCommandHeader, + pub header: PandaCommandHeader, pub status: GetterSetterStatus, } -pub type AutomaticErrorRecoveryRequestWithHeader = RobotCommandHeader; +pub type AutomaticErrorRecoveryRequestWithHeader = PandaCommandHeader; #[derive(Serialize, Deserialize, Debug)] pub struct AutomaticErrorRecoveryResponse { - pub header: RobotCommandHeader, + pub header: PandaCommandHeader, pub status: AutomaticErrorRecoveryStatus, } @@ -575,7 +635,7 @@ pub struct LoadModelLibraryRequest { #[derive(Serialize, Deserialize, Debug, Copy, Clone)] #[repr(packed)] pub struct LoadModelLibraryRequestWithHeader { - pub header: RobotCommandHeader, + pub header: PandaCommandHeader, pub request: LoadModelLibraryRequest, } @@ -587,21 +647,50 @@ impl MessageCommand for LoadModelLibraryRequestWithHeader { #[derive(Serialize, Deserialize, Debug)] pub struct LoadModelLibraryResponse { - pub header: RobotCommandHeader, + pub header: PandaCommandHeader, pub status: LoadModelLibraryStatus, } #[derive(Serialize, Deserialize, Debug, Copy, Clone)] #[repr(packed)] -pub struct RobotCommandHeader { - pub command: RobotCommandEnum, +pub struct PandaCommandHeader { + pub command: PandaCommandEnum, + pub command_id: u32, + pub size: u32, +} + + +impl Default for PandaCommandHeader { + fn default() -> Self { + PandaCommandHeader{ + command: PandaCommandEnum::Connect, + command_id: 0, + size: 0 + } + } +} + +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 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 +698,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/utils.rs b/src/utils.rs index 77e79bd..5d38d21 100644 --- a/src/utils.rs +++ b/src/utils.rs @@ -193,7 +193,7 @@ impl MotionGenerator { #[cfg(test)] mod test { - use crate::{array_to_isometry, ConvertMotion, MotionGenerator, PandaState}; + use crate::{array_to_isometry, ConvertMotion, Finishable, MotionGenerator, PandaState}; use nalgebra::Rotation3; use std::time::Duration; From 3cffb5f0a81032aa1c21f7e22b6ddf1b594ea079 Mon Sep 17 00:00:00 2001 From: Marco Boneberger Date: Fri, 11 Nov 2022 15:25:46 +0100 Subject: [PATCH 04/63] gneric robot implementation --- examples/generate_cartesian_pose_motion.rs | 4 +- src/exception.rs | 3 +- src/gripper/types.rs | 2 +- src/network.rs | 42 +- src/robot.rs | 17 +- src/robot/control_types.rs | 1 - src/robot/logger.rs | 21 +- src/robot/robot_impl.rs | 888 ++++++--------------- src/robot/robot_state.rs | 21 +- src/robot/types.rs | 21 + 10 files changed, 342 insertions(+), 678 deletions(-) diff --git a/examples/generate_cartesian_pose_motion.rs b/examples/generate_cartesian_pose_motion.rs index 6f44562..1e25174 100644 --- a/examples/generate_cartesian_pose_motion.rs +++ b/examples/generate_cartesian_pose_motion.rs @@ -23,7 +23,7 @@ struct CommandLineArguments { fn main() -> FrankaResult<()> { let address = CommandLineArguments::parse(); - let mut robot = FR3::new(address.franka_ip.as_str(), None, None)?; + let mut robot = Panda::new(address.franka_ip.as_str(), 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..."); @@ -46,7 +46,7 @@ fn main() -> FrankaResult<()> { println!("Finished moving to initial joint configuration."); let mut initial_pose = CartesianPose::new([0.0; 16], None); let mut time = 0.; - let callback = |state: &FR3State, time_step: &Duration| -> CartesianPose { + let callback = |state: &PandaState, time_step: &Duration| -> CartesianPose { time += time_step.as_secs_f64(); if time == 0. { initial_pose.O_T_EE = state.O_T_EE_c; diff --git a/src/exception.rs b/src/exception.rs index 9e2a4fd..60c8b95 100644 --- a/src/exception.rs +++ b/src/exception.rs @@ -4,6 +4,7 @@ //! Contains exception and Result definitions use crate::robot::logger::Record; use thiserror::Error; +use crate::PandaState; /// Represents all kind of errors which correspond to the franka::Exception in the C++ version of /// this library @@ -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, }, diff --git a/src/gripper/types.rs b/src/gripper/types.rs index 39e6ae9..a203c54 100644 --- a/src/gripper/types.rs +++ b/src/gripper/types.rs @@ -48,7 +48,7 @@ impl GripperStateIntern { Duration::from_millis(self.message_id as u64) } } - +// TODO is static a problem? pub trait CommandHeader : Debug + DeserializeOwned + 'static { fn get_command_id(&self) -> u32; fn get_size(&self) -> u32; diff --git a/src/network.rs b/src/network.rs index f8b1c03..6b6ad86 100644 --- a/src/network.rs +++ b/src/network.rs @@ -24,13 +24,14 @@ use nix::sys::socket::setsockopt; use nix::sys::socket::sockopt::{KeepAlive, TcpKeepCount, TcpKeepIdle, TcpKeepInterval}; use serde::de::DeserializeOwned; -use serde::Serialize; +use serde::{Deserialize, Serialize}; use crate::exception::{FrankaException, FrankaResult}; use crate::{gripper, GripperState, PandaState}; use crate::gripper::types::{CommandHeader, GripperCommandEnum, GripperCommandHeader}; -use crate::robot::robot_state::FR3State; +use crate::robot::robot_state::{FR3State, RobotState}; use crate::robot::service_types::{FR3CommandEnum, FR3CommandHeader, LoadModelLibraryResponse, LoadModelLibraryStatus, PandaCommandEnum, PandaCommandHeader}; +use crate::robot::types::{RobotStateIntern, PandaStateIntern}; const CLIENT: Token = Token(1); @@ -40,15 +41,21 @@ pub enum NetworkType { Gripper, } -pub trait RobotData { +pub trait DeviceData { type CommandHeader: CommandHeader; type CommandEnum; - type State; - fn create_header(command_id: &mut u32, command: Self::CommandEnum, size: usize, ) -> Self::CommandHeader; +} + +pub trait RobotData : DeviceData { + type DeviceData: DeviceData; + type State:RobotState + From; + type StateIntern: Debug + DeserializeOwned + Serialize + RobotStateIntern + 'static; + + } @@ -64,11 +71,9 @@ pub struct GripperData{ } -impl RobotData for PandaData { +impl DeviceData for PandaData { type CommandHeader = PandaCommandHeader; type CommandEnum = PandaCommandEnum; - type State = PandaState; - 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; @@ -76,22 +81,29 @@ impl RobotData for PandaData { } } -impl RobotData for FR3Data { +impl RobotData for PandaData { + type State = PandaState; + type DeviceData = Self; + type StateIntern = PandaStateIntern; +} +impl DeviceData for FR3Data { type CommandHeader = FR3CommandHeader; type CommandEnum = FR3CommandEnum; - type State = FR3State; - 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 } } +impl RobotData for FR3Data { + type DeviceData = Self; + type State = FR3State; + type StateIntern = PandaStateIntern; // todo create fr3stateintern type +} -impl RobotData for GripperData { +impl DeviceData for GripperData { type CommandHeader = GripperCommandHeader; type CommandEnum = GripperCommandEnum; - type State = GripperState; fn create_header(command_id: &mut u32, command: Self::CommandEnum, size: usize) -> Self::CommandHeader { let header = GripperCommandHeader::new(command, *command_id, size as u32); @@ -104,7 +116,7 @@ pub trait MessageCommand { fn get_command_message_id(&self) -> u32; } -pub struct Network { +pub struct Network { tcp_socket: TcpStream, udp_socket: UdpSocket, udp_server_address: SocketAddr, @@ -123,7 +135,7 @@ pub struct Network { data : PhantomData, } -impl Network { +impl Network { pub fn new( network_type: NetworkType, franka_address: &str, diff --git a/src/robot.rs b/src/robot.rs index 3049944..587c5bd 100644 --- a/src/robot.rs +++ b/src/robot.rs @@ -18,7 +18,7 @@ use crate::robot::control_types::{ use crate::robot::low_pass_filter::{DEFAULT_CUTOFF_FREQUENCY, MAX_CUTOFF_FREQUENCY}; use crate::robot::motion_generator_traits::MotionGeneratorTrait; use crate::robot::robot_control::RobotControl; -use crate::robot::robot_impl::{RobotImplPanda, RobotImplementation, RobotImplFR3}; +use crate::robot::robot_impl::{RobotImplementation, RobotImplGeneric}; use crate::robot::service_types::{ AutomaticErrorRecoveryRequestWithHeader, AutomaticErrorRecoveryResponse, AutomaticErrorRecoveryStatus, GetCartesianLimitRequest, GetCartesianLimitRequestWithHeader, @@ -241,13 +241,13 @@ pub trait Robot where CartesianPose: ConvertMotion<::State1>{ /// 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 Panda { - robimpl: RobotImplPanda, + robimpl: RobotImplGeneric, } impl Robot for Panda { type Data = PandaData; type State1 = PandaState; - type Rob = RobotImplPanda; + type Rob = RobotImplGeneric; fn get_net(&mut self) -> &mut Network{ &mut self.robimpl.network } @@ -257,13 +257,13 @@ impl Robot for Panda { } pub struct FR3 { - robimpl: RobotImplFR3, + robimpl: RobotImplGeneric, } impl Robot for FR3 { type Data = FR3Data; type State1 = FR3State; - type Rob = RobotImplFR3; + type Rob = RobotImplGeneric; fn get_net(&mut self) -> &mut Network { &mut self.robimpl.network } @@ -288,7 +288,7 @@ impl FR3{ message: "Connection could not be established".to_string(), })?; Ok(FR3 { - robimpl: RobotImplFR3::new(network, log_size, realtime_config)?, + robimpl: ::Rob::new(network, log_size, realtime_config)?, }) } } @@ -331,7 +331,7 @@ impl Panda { message: "Connection could not be established".to_string(), })?; Ok(Panda { - robimpl: RobotImplPanda::new(network, log_size, realtime_config)?, + robimpl: ::Rob::new(network, log_size, realtime_config)?, }) } /// Starts a loop for reading the current robot state. @@ -1238,7 +1238,8 @@ impl Panda { /// * [`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) + // self.robimpl.load_model(persistent) // todo care about model + FrankaResult::Err(FrankaException::ModelException { message: "fuck".to_string() }) } /// Returns the software version reported by the connected server. /// diff --git a/src/robot/control_types.rs b/src/robot/control_types.rs index cf4dd6d..ee93ff0 100644 --- a/src/robot/control_types.rs +++ b/src/robot/control_types.rs @@ -23,7 +23,6 @@ use crate::robot::service_types::MoveMotionGeneratorMode; use crate::robot::types::MotionGeneratorCommand; use crate::utils::Vector7; use nalgebra::{Isometry3, Vector6}; -use crate::robot::robot_impl::RobotImplFR3; /// Available controller modes for a [`Robot`](`crate::Robot`) pub enum ControllerMode { diff --git a/src/robot/logger.rs b/src/robot/logger.rs index b700074..5ce601b 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::PandaState; +use crate::robot::robot_state::{PandaState, RobotState}; 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: PandaState, + 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()) } } -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: &PandaState, 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/robot_impl.rs b/src/robot/robot_impl.rs index 7a930dc..b4998e8 100644 --- a/src/robot/robot_impl.rs +++ b/src/robot/robot_impl.rs @@ -5,6 +5,7 @@ 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::network::DeviceData; use crate::network::{FR3Data, Network, PandaData, RobotData}; use crate::robot::control_types::RealtimeConfig; use crate::robot::errors::FrankaErrors; @@ -12,116 +13,257 @@ use crate::robot::logger::{Logger, Record}; use crate::robot::robot_control::RobotControl; use crate::robot::robot_state::{FR3State, PandaState, RobotState}; -use crate::robot::service_types::{ConnectRequest, ConnectRequestWithHeader, ConnectResponse, ConnectStatus, MoveControllerMode, MoveDeviation, MoveMotionGeneratorMode, MoveRequest, MoveRequestWithHeader, MoveResponse, MoveStatus, PandaCommandEnum, PandaCommandHeader, StopMoveRequestWithHeader, StopMoveResponse, StopMoveStatus, VERSION}; -use crate::robot::types::{ - ControllerCommand, ControllerMode, MotionGeneratorCommand, MotionGeneratorMode, RobotCommand, - RobotMode, PandaStateIntern, +use crate::robot::service_types::{ + ConnectRequest, ConnectRequestWithHeader, ConnectResponse, ConnectStatus, MoveControllerMode, + MoveDeviation, MoveMotionGeneratorMode, MoveRequest, MoveRequestWithHeader, MoveResponse, + MoveStatus, PandaCommandEnum, PandaCommandHeader, StopMoveRequestWithHeader, StopMoveResponse, + StopMoveStatus, VERSION, }; +use crate::robot::types::{ControllerCommand, ControllerMode, RobotStateIntern, MotionGeneratorCommand, MotionGeneratorMode, PandaStateIntern, RobotCommand, RobotMode}; use std::fs::remove_file; use std::path::Path; -pub trait RobotImplementation : RobotControl { - type State:RobotState; +pub trait RobotImplementation: RobotControl { + type State: RobotState; fn update2( &mut self, motion_command: Option<&MotionGeneratorCommand>, control_command: Option<&ControllerCommand>, - ) -> FrankaResult; + ) -> FrankaResult; // todo remove commented out code + // { + // let robot_command = self.send_robot_command(motion_command, control_command)?; + // let state = Self::State::from(self.receive_robot_state()?); + // if let Some(command) = robot_command { + // self.logger.log(&state, &command); + // } + // Ok(state) + // } fn read_once2(&mut self) -> FrankaResult; fn load_model2(&mut self, persistent: bool) -> FrankaResult; fn server_version2(&self) -> u16; } -impl RobotImplementation for RobotImplPanda { - type State = PandaState; +pub struct RobotImplGeneric { + pub network: Network<(Data::DeviceData)>, + logger: Logger, + realtime_config: RealtimeConfig, + ri_version: Option, + motion_generator_mode: Option, + current_move_motion_generator_mode: MotionGeneratorMode, + controller_mode: ControllerMode, + current_move_controller_mode: Option, + message_id: u64, +} - fn update2(&mut self, motion_command: Option<&MotionGeneratorCommand>, control_command: Option<&ControllerCommand>) -> FrankaResult { +impl RobotControl for RobotImplGeneric { + type State2 = Data::State; + + fn start_motion( + &mut self, + controller_mode: MoveControllerMode, + motion_generator_mode: MoveMotionGeneratorMode, + maximum_path_deviation: &MoveDeviation, + maximum_goal_deviation: &MoveDeviation, + ) -> FrankaResult { + if self.motion_generator_running() || self.controller_running() { + panic!("libfranka-rs robot: Attempted to start multiple motions!"); + } + self.current_move_motion_generator_mode = match motion_generator_mode { + MoveMotionGeneratorMode::JointPosition => MotionGeneratorMode::JointPosition, + MoveMotionGeneratorMode::JointVelocity => MotionGeneratorMode::JointVelocity, + MoveMotionGeneratorMode::CartesianPosition => MotionGeneratorMode::CartesianPosition, + MoveMotionGeneratorMode::CartesianVelocity => MotionGeneratorMode::CartesianVelocity, + }; + self.current_move_controller_mode = match controller_mode { + MoveControllerMode::JointImpedance => Some(ControllerMode::JointImpedance), + MoveControllerMode::ExternalController => Some(ControllerMode::ExternalController), + MoveControllerMode::CartesianImpedance => Some(ControllerMode::CartesianImpedance), + }; + let move_command_id = self.execute_move_command( + &controller_mode, + &motion_generator_mode, + maximum_path_deviation, + maximum_goal_deviation, + )?; + + while self.motion_generator_mode.unwrap() != self.current_move_motion_generator_mode + || Some(self.controller_mode) != self.current_move_controller_mode + { + match self + .network + .tcp_receive_response(move_command_id, |x| handle_command_response_move(&x)) + { + Ok(received_message) => { + if received_message { + break; + } + } + Err(FrankaException::CommandException { message }) => { + return Err(FrankaException::ControlException { + log: None, + error: message, + }); + } + _ => { + panic!("this should be an command exception but it is not") + } + } + + let _robot_state = self.update(None, None)?; + } + self.logger.flush(); + Ok(move_command_id) + } + #[allow(unused_must_use)] + fn finish_motion( + &mut self, + motion_id: u32, + motion_command: Option<&MotionGeneratorCommand>, + control_command: Option<&ControllerCommand>, + ) -> FrankaResult<()> { + if !self.motion_generator_running() && !self.controller_running() { + self.current_move_motion_generator_mode = MotionGeneratorMode::Idle; + self.current_move_controller_mode = Some(ControllerMode::Other); + return Ok(()); + } + if motion_command.is_none() { + return Err(FrankaException::ControlException { + log: None, + error: String::from("libfranka-rs robot: No motion generator command given!"), + }); + } + let mut motion_finished_command = *motion_command.unwrap(); + motion_finished_command.motion_generation_finished = true; + let mut robot_state = None; + while self.motion_generator_running() || self.controller_running() { + 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.get_last_motion_errors(), + self.logger.flush(), + )); + } + match handle_command_response_move(&response) { + Ok(_) => {} + Err(FrankaException::CommandException { message }) => { + return Err(create_control_exception( + message, + &response.status, + &robot_state.get_last_motion_errors(), + self.logger.flush(), + )); + } + _ => { + panic!("this should be an command exception but it is not") + } + } + self.current_move_motion_generator_mode = MotionGeneratorMode::Idle; + self.current_move_controller_mode = Some(ControllerMode::Other); + Ok(()) + } + + // TODO exception handling + fn cancel_motion(&mut self, motion_id: u32) { + self.execute_stop_command() + .expect("error while canceling motion"); //todo handle Network exception. But it is not that important since we already have an error anyways + let mut _robot_state = self.receive_robot_state(); + while self.motion_generator_running() || self.controller_running() { + _robot_state = self.receive_robot_state(); + } + + // comment from libfranka devs: + // Ignore Move response. + // TODO (FWA): It is not guaranteed that the Move response won't come later + + self.network + .tcp_receive_response(motion_id, |_x: MoveResponse| 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); + } + + fn update( + &mut self, + motion_command: Option<&MotionGeneratorCommand>, + control_command: Option<&ControllerCommand>, + ) -> FrankaResult { let robot_command = self.send_robot_command(motion_command, control_command)?; - let state = PandaState::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); + self.logger.log(&PandaState::default(), &command); // todo log properly } Ok(state) } - fn read_once2(&mut self) -> FrankaResult { - while self.network.udp_receive::().is_some() {} - Ok(PandaState::from(self.receive_robot_state()?)) + fn realtime_config(&self) -> RealtimeConfig { + self.realtime_config } - fn load_model2(&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"); + + fn throw_on_motion_error( + &mut self, + robot_state: &Data::State, + motion_id: u32, + ) -> FrankaResult<()> { + 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); + return match result { + Err(error) => Err(create_control_exception( + error.to_string(), + &response.status, + &robot_state.get_last_motion_errors(), + self.logger.flush(), + )), + Ok(_) => panic!("Unexpected reply to a Move command"), + }; } - Ok(model) - } - fn server_version2(&self) -> u16 { - self.ri_version.unwrap() + Ok(()) } } -impl RobotImplementation for RobotImplFR3 { - type State = FR3State; +impl RobotImplementation for RobotImplGeneric { + type State = Data::State; fn update2(&mut self, motion_command: Option<&MotionGeneratorCommand>, control_command: Option<&ControllerCommand>) -> FrankaResult { let robot_command = self.send_robot_command(motion_command, control_command)?; - let state = FR3State::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); // todo fix logger + self.logger.log(&PandaState::default(), &command); // todo log properly } Ok(state) } fn read_once2(&mut self) -> FrankaResult { - while self.network.udp_receive::().is_some() {} - Ok(FR3State::from(self.receive_robot_state()?)) + while self.network.udp_receive::().is_some() {} + Ok(Data::State::from(self.receive_robot_state()?)) } fn load_model2(&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::download2(&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) + /// todo take care of model + // 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) + FrankaResult::Err(FrankaException::ModelException { message: "fuck".to_string() }) } fn server_version2(&self) -> u16 { self.ri_version.unwrap() } } -pub struct RobotImplPanda { - pub network: Network, - logger: Logger, - realtime_config: RealtimeConfig, - ri_version: Option, - motion_generator_mode: Option, - current_move_motion_generator_mode: MotionGeneratorMode, - controller_mode: ControllerMode, - current_move_controller_mode: Option, - message_id: u64, -} - -pub struct RobotImplFR3 { - pub network: Network, - logger: Logger, - realtime_config: RealtimeConfig, - ri_version: Option, - motion_generator_mode: Option, - current_move_motion_generator_mode: MotionGeneratorMode, - controller_mode: ControllerMode, - current_move_controller_mode: Option, - message_id: u64, -} - fn handle_command_response_move(response: &MoveResponse) -> Result<(), FrankaException> { match response.status { MoveStatus::Success => Ok(()), @@ -174,16 +316,18 @@ fn handle_command_response_stop(response: &StopMoveResponse) -> Result<(), Frank } } -impl RobotImplPanda { + + +impl RobotImplGeneric { pub fn new( - network: Network, + 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 = RobotImplPanda { + let mut robot_impl = RobotImplGeneric { network, logger, realtime_config, @@ -197,14 +341,16 @@ impl RobotImplPanda { robot_impl.connect_robot()?; let state = robot_impl .network - .udp_blocking_receive::() + .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:PandaData::create_header(&mut self.network.command_id, + header: PandaData::create_header( + &mut self.network.command_id, PandaCommandEnum::Connect, size_of::(), ), @@ -224,32 +370,32 @@ impl RobotImplPanda { }), } } - fn update_state_with(&mut self, robot_state: &PandaStateIntern) { - self.motion_generator_mode = Some(robot_state.motion_generator_mode); - self.controller_mode = robot_state.controller_mode; - self.message_id = robot_state.message_id; + 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(); } - pub fn read_once(&mut self) -> FrankaResult { - while self.network.udp_receive::().is_some() {} - Ok(PandaState::from(self.receive_robot_state()?)) + pub fn read_once(&mut self) -> FrankaResult { + while self.network.udp_receive::().is_some() {} + Ok(Data::State::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::(); + 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 + if received_state.unwrap().get_message_id() > match latest_accepted_state { - Some(s) => s.message_id, - None => self.message_id, - } + Some(s) => s.get_message_id(), + None => self.message_id, + } { latest_accepted_state = Some(received_state.unwrap()); } - received_state = self.network.udp_receive::(); + 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 { + if received_state.unwrap().get_message_id() > self.message_id { latest_accepted_state = received_state; } } @@ -315,18 +461,19 @@ impl RobotImplPanda { 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) - } + /// todo take care of model + // 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() } @@ -368,547 +515,10 @@ impl RobotImplPanda { } } -impl RobotImplFR3 { - 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 = RobotImplFR3 { - 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_panda( - PandaCommandEnum::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: &PandaStateIntern) { - 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(FR3State::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), - }), - }; - } - } - - 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::download2(&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_panda( - PandaCommandEnum::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_panda( - PandaCommandEnum::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 RobotImplPanda { - type State2 = PandaState; - fn start_motion( - &mut self, - controller_mode: MoveControllerMode, - motion_generator_mode: MoveMotionGeneratorMode, - maximum_path_deviation: &MoveDeviation, - maximum_goal_deviation: &MoveDeviation, - ) -> FrankaResult { - if self.motion_generator_running() || self.controller_running() { - panic!("libfranka-rs robot: Attempted to start multiple motions!"); - } - self.current_move_motion_generator_mode = match motion_generator_mode { - MoveMotionGeneratorMode::JointPosition => MotionGeneratorMode::JointPosition, - MoveMotionGeneratorMode::JointVelocity => MotionGeneratorMode::JointVelocity, - MoveMotionGeneratorMode::CartesianPosition => MotionGeneratorMode::CartesianPosition, - MoveMotionGeneratorMode::CartesianVelocity => MotionGeneratorMode::CartesianVelocity, - }; - self.current_move_controller_mode = match controller_mode { - MoveControllerMode::JointImpedance => Some(ControllerMode::JointImpedance), - MoveControllerMode::ExternalController => Some(ControllerMode::ExternalController), - MoveControllerMode::CartesianImpedance => Some(ControllerMode::CartesianImpedance), - }; - let move_command_id = self.execute_move_command( - &controller_mode, - &motion_generator_mode, - maximum_path_deviation, - maximum_goal_deviation, - )?; - while self.motion_generator_mode.unwrap() != self.current_move_motion_generator_mode - || Some(self.controller_mode) != self.current_move_controller_mode - { - match self - .network - .tcp_receive_response(move_command_id, |x| handle_command_response_move(&x)) - { - Ok(received_message) => { - if received_message { - break; - } - } - Err(FrankaException::CommandException { message }) => { - return Err(FrankaException::ControlException { - log: None, - error: message, - }); - } - _ => { - panic!("this should be an command exception but it is not") - } - } - let _robot_state = self.update(None, None)?; - } - self.logger.flush(); - Ok(move_command_id) - } - #[allow(unused_must_use)] - fn finish_motion( - &mut self, - motion_id: u32, - motion_command: Option<&MotionGeneratorCommand>, - control_command: Option<&ControllerCommand>, - ) -> FrankaResult<()> { - if !self.motion_generator_running() && !self.controller_running() { - self.current_move_motion_generator_mode = MotionGeneratorMode::Idle; - self.current_move_controller_mode = Some(ControllerMode::Other); - return Ok(()); - } - if motion_command.is_none() { - return Err(FrankaException::ControlException { - log: None, - error: String::from("libfranka-rs robot: No motion generator command given!"), - }); - } - let mut motion_finished_command = *motion_command.unwrap(); - motion_finished_command.motion_generation_finished = true; - let mut robot_state = None; - while self.motion_generator_running() || self.controller_running() { - 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) { - Ok(_) => {} - Err(FrankaException::CommandException { message }) => { - return Err(create_control_exception( - message, - &response.status, - &robot_state.last_motion_errors, - self.logger.flush(), - )); - } - _ => { - panic!("this should be an command exception but it is not") - } - } - self.current_move_motion_generator_mode = MotionGeneratorMode::Idle; - self.current_move_controller_mode = Some(ControllerMode::Other); - Ok(()) - } - // TODO exception handling - fn cancel_motion(&mut self, motion_id: u32) { - self.execute_stop_command() - .expect("error while canceling motion"); //todo handle Network exception. But it is not that important since we already have an error anyways - let mut _robot_state = self.receive_robot_state(); - while self.motion_generator_running() || self.controller_running() { - _robot_state = self.receive_robot_state(); - } - - // comment from libfranka devs: - // Ignore Move response. - // TODO (FWA): It is not guaranteed that the Move response won't come later - - self.network - .tcp_receive_response(motion_id, |_x: MoveResponse| 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); - } - - fn update( - &mut self, - motion_command: Option<&MotionGeneratorCommand>, - control_command: Option<&ControllerCommand>, - ) -> FrankaResult { - let robot_command = self.send_robot_command(motion_command, control_command)?; - let state = PandaState::from(self.receive_robot_state()?); - if let Some(command) = robot_command { - self.logger.log(&state, &command); - } - Ok(state) - } - - fn realtime_config(&self) -> RealtimeConfig { - self.realtime_config - } - - fn throw_on_motion_error( - &mut self, - robot_state: &PandaState, - motion_id: u32, - ) -> FrankaResult<()> { - if robot_state.robot_mode != RobotMode::Move - || 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); - return match result { - Err(error) => Err(create_control_exception( - error.to_string(), - &response.status, - &robot_state.last_motion_errors, - self.logger.flush(), - )), - Ok(_) => panic!("Unexpected reply to a Move command"), - }; - } - Ok(()) - } -} - -impl RobotControl for RobotImplFR3 { - type State2 = FR3State; - - fn start_motion( - &mut self, - controller_mode: MoveControllerMode, - motion_generator_mode: MoveMotionGeneratorMode, - maximum_path_deviation: &MoveDeviation, - maximum_goal_deviation: &MoveDeviation, - ) -> FrankaResult { - if self.motion_generator_running() || self.controller_running() { - panic!("libfranka-rs robot: Attempted to start multiple motions!"); - } - self.current_move_motion_generator_mode = match motion_generator_mode { - MoveMotionGeneratorMode::JointPosition => MotionGeneratorMode::JointPosition, - MoveMotionGeneratorMode::JointVelocity => MotionGeneratorMode::JointVelocity, - MoveMotionGeneratorMode::CartesianPosition => MotionGeneratorMode::CartesianPosition, - MoveMotionGeneratorMode::CartesianVelocity => MotionGeneratorMode::CartesianVelocity, - }; - self.current_move_controller_mode = match controller_mode { - MoveControllerMode::JointImpedance => Some(ControllerMode::JointImpedance), - MoveControllerMode::ExternalController => Some(ControllerMode::ExternalController), - MoveControllerMode::CartesianImpedance => Some(ControllerMode::CartesianImpedance), - }; - let move_command_id = self.execute_move_command( - &controller_mode, - &motion_generator_mode, - maximum_path_deviation, - maximum_goal_deviation, - )?; - - while self.motion_generator_mode.unwrap() != self.current_move_motion_generator_mode - || Some(self.controller_mode) != self.current_move_controller_mode - { - match self - .network - .tcp_receive_response(move_command_id, |x| handle_command_response_move(&x)) - { - Ok(received_message) => { - if received_message { - break; - } - } - Err(FrankaException::CommandException { message }) => { - return Err(FrankaException::ControlException { - log: None, - error: message, - }); - } - _ => { - panic!("this should be an command exception but it is not") - } - } - - let _robot_state = self.update(None, None)?; - } - self.logger.flush(); - Ok(move_command_id) - } - #[allow(unused_must_use)] - fn finish_motion( - &mut self, - motion_id: u32, - motion_command: Option<&MotionGeneratorCommand>, - control_command: Option<&ControllerCommand>, - ) -> FrankaResult<()> { - if !self.motion_generator_running() && !self.controller_running() { - self.current_move_motion_generator_mode = MotionGeneratorMode::Idle; - self.current_move_controller_mode = Some(ControllerMode::Other); - return Ok(()); - } - if motion_command.is_none() { - return Err(FrankaException::ControlException { - log: None, - error: String::from("libfranka-rs robot: No motion generator command given!"), - }); - } - let mut motion_finished_command = *motion_command.unwrap(); - motion_finished_command.motion_generation_finished = true; - let mut robot_state = None; - while self.motion_generator_running() || self.controller_running() { - 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) { - Ok(_) => {} - Err(FrankaException::CommandException { message }) => { - return Err(create_control_exception( - message, - &response.status, - &robot_state.last_motion_errors, - self.logger.flush(), - )); - } - _ => { - panic!("this should be an command exception but it is not") - } - } - self.current_move_motion_generator_mode = MotionGeneratorMode::Idle; - self.current_move_controller_mode = Some(ControllerMode::Other); - Ok(()) - } - - // TODO exception handling - fn cancel_motion(&mut self, motion_id: u32) { - self.execute_stop_command() - .expect("error while canceling motion"); //todo handle Network exception. But it is not that important since we already have an error anyways - let mut _robot_state = self.receive_robot_state(); - while self.motion_generator_running() || self.controller_running() { - _robot_state = self.receive_robot_state(); - } - - // comment from libfranka devs: - // Ignore Move response. - // TODO (FWA): It is not guaranteed that the Move response won't come later - - self.network - .tcp_receive_response(motion_id, |_x: MoveResponse| 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); - } - - fn update( - &mut self, - motion_command: Option<&MotionGeneratorCommand>, - control_command: Option<&ControllerCommand>, - ) -> FrankaResult { - let robot_command = self.send_robot_command(motion_command, control_command)?; - let state = FR3State::from(self.receive_robot_state()?); - if let Some(command) = robot_command { - // self.logger.log(&state, &command); TODO(care about logger) - } - Ok(state) - } - - fn realtime_config(&self) -> RealtimeConfig { - self.realtime_config - } - - fn throw_on_motion_error( - &mut self, - robot_state: &Self::State2, - motion_id: u32, - ) -> FrankaResult<()> { - if robot_state.robot_mode != RobotMode::Move - || 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); - return match result { - Err(error) => Err(create_control_exception( - error.to_string(), - &response.status, - &robot_state.last_motion_errors, - self.logger.flush(), - )), - Ok(_) => panic!("Unexpected reply to a Move command"), - }; - } - Ok(()) - } -} // impl MotionGeneratorTrait for RobotImpl { // fn get_motion_generator_mode() -> MoveMotionGeneratorMode { @@ -920,7 +530,7 @@ fn create_control_exception( message: String, move_status: &MoveStatus, reflex_reasons: &FrankaErrors, - log: Vec, + log: Vec>, ) -> FrankaException { let mut exception_string = String::from(&message); if move_status == &MoveStatus::ReflexAborted { diff --git a/src/robot/robot_state.rs b/src/robot/robot_state.rs index 25f2f46..fb24237 100644 --- a/src/robot/robot_state.rs +++ b/src/robot/robot_state.rs @@ -2,16 +2,20 @@ // 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, PandaStateIntern}; use nalgebra::{Matrix3, Vector3}; +use serde::Serialize; use crate::robot::FR3; -pub trait RobotState { +pub trait RobotState : Clone + Debug { fn get_time(&self) -> Duration; fn get_tau_J_d(&self) -> [f64;7]; + fn get_last_motion_errors(&self) -> &FrankaErrors; + fn is_moving(&self) -> bool; } /// Describes the robot state. @@ -815,6 +819,14 @@ impl RobotState for PandaState { 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 + } } impl RobotState for FR3State { @@ -826,6 +838,13 @@ impl RobotState for FR3State { 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 + } } #[cfg(test)] diff --git a/src/robot/types.rs b/src/robot/types.rs index ff2b2e3..22f39da 100644 --- a/src/robot/types.rs +++ b/src/robot/types.rs @@ -6,6 +6,7 @@ use crate::robot::types::RobotMode::Other; use serde::Deserialize; use serde::Serialize; use serde_repr::{Deserialize_repr, Serialize_repr}; +use crate::network::MessageCommand; #[derive(Serialize_repr, Deserialize_repr, Debug, Copy, Clone, PartialEq)] #[repr(u8)] @@ -96,6 +97,26 @@ pub struct PandaStateIntern { pub control_command_success_rate: f64, } +pub trait RobotStateIntern : Copy + Clone + PartialEq{ + fn get_message_id(&self) -> u64; + fn get_motion_generator_mode(&self) -> MotionGeneratorMode; + fn get_controller_mode(&self) -> ControllerMode; +} + +impl RobotStateIntern 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 { PandaStateIntern { From adecd1c3c5117357cb9f91248d4bb18c9a42f5cb Mon Sep 17 00:00:00 2001 From: Marco Boneberger Date: Fri, 2 Dec 2022 13:27:53 +0100 Subject: [PATCH 05/63] make model library generic --- examples/cartesian_impedance_control.rs | 12 +- src/lib.rs | 4 +- src/model.rs | 430 +++++++++++++++- src/model/library_downloader.rs | 66 +-- src/network.rs | 107 ++-- src/robot.rs | 621 ++++++++++++------------ src/robot/control_loop.rs | 33 +- src/robot/robot_impl.rs | 64 +-- src/robot/service_types.rs | 14 + 9 files changed, 884 insertions(+), 467 deletions(-) diff --git a/examples/cartesian_impedance_control.rs b/examples/cartesian_impedance_control.rs index 0aac5c6..043b6ff 100644 --- a/examples/cartesian_impedance_control.rs +++ b/examples/cartesian_impedance_control.rs @@ -2,14 +2,14 @@ // Licensed under the EUPL-1.2-or-later use clap::Parser; -use franka::Frame; +use franka::{Frame, Panda, PandaState, RobotModel}; 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 franka::robot::{FR3, Robot}; +use franka::robot::robot_state::FR3State; /// An example showing a simple cartesian impedance controller without inertia shaping /// that renders a spring damper system where the equilibrium is the initial configuration. @@ -43,7 +43,7 @@ fn main() -> FrankaResult<()> { 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 mut robot = FR3::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! @@ -66,7 +66,7 @@ fn main() -> FrankaResult<()> { println!("Press Enter to continue..."); std::io::stdin().read_line(&mut String::new()).unwrap(); let result = robot.control_torques( - |state: &RobotState, _step: &Duration| -> Torques { + |state: &FR3State, _step: &Duration| -> Torques { 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); @@ -98,7 +98,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/src/lib.rs b/src/lib.rs index 971a317..5097126 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -175,7 +175,9 @@ pub use exception::FrankaResult; pub use gripper::gripper_state::GripperState; pub use gripper::Gripper; pub use model::Frame; -pub use model::Model; +pub use model::PandaModel; +pub use model::RobotModel; +pub use model::FR3Model; pub use robot::control_types::*; pub use robot::low_pass_filter::DEFAULT_CUTOFF_FREQUENCY; pub use robot::low_pass_filter::MAX_CUTOFF_FREQUENCY; diff --git a/src/model.rs b/src/model.rs index 6a266cb..d91a467 100644 --- a/src/model.rs +++ b/src/model.rs @@ -5,7 +5,7 @@ use nalgebra::Matrix4; use crate::model::model_library::ModelLibrary; -use crate::robot::robot_state::PandaState; +use crate::robot::robot_state::{FR3State, PandaState, RobotState}; use crate::FrankaResult; use std::fmt; use std::path::Path; @@ -64,13 +64,407 @@ impl fmt::Display for Frame { } } +pub trait RobotModel { + type State: RobotState; + fn new>(model_filename: S, libm_filename: Option<&Path>) -> FrankaResult where Self: Sized; + + fn pose( + &self, + frame: &Frame, + q: &[f64; 7], + F_T_EE: &[f64; 16], + EE_T_K: &[f64; 16], + ) -> [f64; 16]; + + fn pose_from_state(&self, frame: &Frame, robot_state: &Self::State) -> [f64; 16]; + fn body_jacobian( + &self, + frame: &Frame, + q: &[f64; 7], + F_T_EE: &[f64; 16], + EE_T_K: &[f64; 16], + ) -> [f64; 42]; + + fn body_jacobian_from_state(&self, frame: &Frame, robot_state: &Self::State) -> [f64; 42]; + fn zero_jacobian( + &self, + frame: &Frame, + q: &[f64; 7], + F_T_EE: &[f64; 16], + EE_T_K: &[f64; 16], + ) -> [f64; 42]; + fn zero_jacobian_from_state(&self, frame: &Frame, robot_state: &Self::State) -> [f64; 42]; + fn mass( + &self, + q: &[f64; 7], + I_total: &[f64; 9], + m_total: f64, + F_x_Ctotal: &[f64; 3], + ) -> [f64; 49]; + fn mass_from_state(&self, robot_state: &Self::State) -> [f64; 49]; + fn coriolis( + &self, + q: &[f64; 7], + dq: &[f64; 7], + I_total: &[f64; 9], + m_total: f64, + F_x_Ctotal: &[f64; 3], + ) -> [f64; 7]; + fn coriolis_from_state(&self, robot_state: &Self::State) -> [f64; 7]; + fn gravity<'a, Grav: Into>>( + &self, + q: &[f64; 7], + m_total: f64, + F_x_Ctotal: &[f64; 3], + gravity_earth: Grav, + ) -> [f64; 7]; + fn gravity_from_state<'a, Grav: Into>>( + &self, + robot_state: &Self::State, + gravity_earth: Grav, + ) -> [f64; 7]; +} + +/// Calculates poses of joints and dynamic properties of the robot. +pub struct PandaModel { + library: ModelLibrary, +} + +#[allow(non_snake_case)] +impl RobotModel for PandaModel { + type State = PandaState; + /// 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`). + /// + /// If you do not have the model you can use the download_model example to download the model + /// # Arguments + /// * `model_filename` - Path to the model. + /// * `libm_filename` - Path to libm.so.6 . You probably do not need to specify the path as it + /// it should be detected automatically. However if you get panics for unresolved symbols or libm could + /// not be found you have to specify the path. On most Ubuntu systems it is in + /// ```text + /// /lib/x86_64-linux-gnu/libm.so.6 + /// ``` + /// It maybe has a different name on your machine but it is not the "libm.so". You can + /// verify that it is the correct libm by checking: + /// ```bash + /// nm -D /lib/x86_64-linux-gnu/libm.so.6 | grep sincos + /// ``` + /// # Errors + /// * ModelException + /// # libm FAQ + /// What is libm? - Libm is the Math library of the C Standard Library. It is what you get when + /// you include in C + /// + /// 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 \ ¯\_(ツ)_/¯ + fn new>( + model_filename: S, + libm_filename: Option<&Path>, + ) -> FrankaResult { + Ok(PandaModel { + library: ModelLibrary::new(model_filename.as_ref(), libm_filename)?, + }) + } + /// 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. + /// * `q` - Joint position. + /// * `F_T_EE` - End effector in flange frame. + /// * `EE_T_K` - Stiffness frame K in the end effector frame. + /// # Return + /// Vectorized 4x4 pose matrix, column-major. + fn pose( + &self, + frame: &Frame, + q: &[f64; 7], + F_T_EE: &[f64; 16], + EE_T_K: &[f64; 16], + ) -> [f64; 16] { + let mut output = [0.; 16]; + match frame { + Frame::Joint1 => self.library.joint1(q, &mut output), + Frame::Joint2 => self.library.joint2(q, &mut output), + Frame::Joint3 => self.library.joint3(q, &mut output), + Frame::Joint4 => self.library.joint4(q, &mut output), + Frame::Joint5 => self.library.joint5(q, &mut output), + Frame::Joint6 => self.library.joint6(q, &mut output), + Frame::Joint7 => self.library.joint7(q, &mut output), + Frame::Flange => self.library.flange(q, &mut output), + Frame::EndEffector => self.library.ee(q, F_T_EE, &mut output), + Frame::Stiffness => { + let tmp: Matrix4 = + Matrix4::from_column_slice(F_T_EE) * Matrix4::from_column_slice(EE_T_K); + let mut stiffness_f_t_ee = [0.; 16]; + tmp.iter() + .enumerate() + .for_each(|(i, &x)| stiffness_f_t_ee[i] = x); + self.library.ee(q, &stiffness_f_t_ee, &mut output) + } + } + 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. + fn pose_from_state(&self, frame: &Frame, robot_state: &Self::State) -> [f64; 16] { + self.pose( + frame, + &robot_state.q, + &robot_state.F_T_EE, + &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. + fn body_jacobian( + &self, + frame: &Frame, + q: &[f64; 7], + F_T_EE: &[f64; 16], + EE_T_K: &[f64; 16], + ) -> [f64; 42] { + let mut output = [0.; 42]; + match frame { + Frame::Joint1 => self.library.body_jacobian_joint1(&mut output), + Frame::Joint2 => self.library.body_jacobian_joint2(q, &mut output), + Frame::Joint3 => self.library.body_jacobian_joint3(q, &mut output), + Frame::Joint4 => self.library.body_jacobian_joint4(q, &mut output), + Frame::Joint5 => self.library.body_jacobian_joint5(q, &mut output), + Frame::Joint6 => self.library.body_jacobian_joint6(q, &mut output), + Frame::Joint7 => self.library.body_jacobian_joint7(q, &mut output), + Frame::Flange => self.library.body_jacobian_flange(q, &mut output), + Frame::EndEffector => self.library.body_jacobian_ee(q, F_T_EE, &mut output), + Frame::Stiffness => { + let tmp: Matrix4 = + Matrix4::from_column_slice(F_T_EE) * Matrix4::from_column_slice(EE_T_K); + let mut stiffness_f_t_ee = [0.; 16]; + tmp.iter() + .enumerate() + .for_each(|(i, &x)| stiffness_f_t_ee[i] = x); + self.library + .body_jacobian_ee(q, &stiffness_f_t_ee, &mut output) + } + } + 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. + fn body_jacobian_from_state(&self, frame: &Frame, robot_state: &Self::State) -> [f64; 42] { + self.body_jacobian( + frame, + &robot_state.q, + &robot_state.F_T_EE, + &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. + fn zero_jacobian( + &self, + frame: &Frame, + q: &[f64; 7], + F_T_EE: &[f64; 16], + EE_T_K: &[f64; 16], + ) -> [f64; 42] { + let mut output = [0.; 42]; + match frame { + Frame::Joint1 => self.library.zero_jacobian_joint1(&mut output), + Frame::Joint2 => self.library.zero_jacobian_joint2(q, &mut output), + Frame::Joint3 => self.library.zero_jacobian_joint3(q, &mut output), + Frame::Joint4 => self.library.zero_jacobian_joint4(q, &mut output), + Frame::Joint5 => self.library.zero_jacobian_joint5(q, &mut output), + Frame::Joint6 => self.library.zero_jacobian_joint6(q, &mut output), + Frame::Joint7 => self.library.zero_jacobian_joint7(q, &mut output), + Frame::Flange => self.library.zero_jacobian_flange(q, &mut output), + Frame::EndEffector => self.library.zero_jacobian_ee(q, F_T_EE, &mut output), + Frame::Stiffness => { + let tmp: Matrix4 = + Matrix4::from_column_slice(F_T_EE) * Matrix4::from_column_slice(EE_T_K); + let mut stiffness_f_t_ee = [0.; 16]; + tmp.iter() + .enumerate() + .for_each(|(i, &x)| stiffness_f_t_ee[i] = x); + self.library + .zero_jacobian_ee(q, &stiffness_f_t_ee, &mut output) + } + } + 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. + fn zero_jacobian_from_state(&self, frame: &Frame, robot_state: &Self::State) -> [f64; 42] { + self.zero_jacobian( + frame, + &robot_state.q, + &robot_state.F_T_EE, + &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. + fn mass( + &self, + q: &[f64; 7], + I_total: &[f64; 9], + m_total: f64, + F_x_Ctotal: &[f64; 3], + ) -> [f64; 49] { + let mut output = [0.; 49]; + self.library + .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. + fn mass_from_state(&self, robot_state: &Self::State) -> [f64; 49] { + self.mass( + &robot_state.q, + &robot_state.I_total, + robot_state.m_total, + &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. + fn coriolis( + &self, + q: &[f64; 7], + dq: &[f64; 7], + I_total: &[f64; 9], + m_total: f64, + F_x_Ctotal: &[f64; 3], + ) -> [f64; 7] { + let mut output = [0.; 7]; + self.library + .coriolis(q, dq, I_total, &m_total, F_x_Ctotal, &mut output); + 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. + fn coriolis_from_state(&self, robot_state: &Self::State) -> [f64; 7] { + self.coriolis( + &robot_state.q, + &robot_state.dq, + &robot_state.I_load, + robot_state.m_total, + &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. + fn gravity<'a, Grav: Into>>( + &self, + q: &[f64; 7], + m_total: f64, + F_x_Ctotal: &[f64; 3], + gravity_earth: Grav, + ) -> [f64; 7] { + let gravity_earth = gravity_earth.into().unwrap_or(&[0., 0., -9.81]); + let mut output = [0.; 7]; + self.library + .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. + fn gravity_from_state<'a, Grav: Into>>( + &self, + robot_state: &Self::State, + gravity_earth: Grav, + ) -> [f64; 7] { + self.gravity( + &robot_state.q, + robot_state.m_total, + &robot_state.F_x_Ctotal, + gravity_earth.into().unwrap_or(&robot_state.O_ddP_O), + ) + } +} + /// Calculates poses of joints and dynamic properties of the robot. -pub struct Model { +pub struct FR3Model { library: ModelLibrary, } #[allow(non_snake_case)] -impl Model { +impl RobotModel for FR3Model { + type State = FR3State; /// 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 @@ -99,11 +493,11 @@ 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>( + fn new>( model_filename: S, libm_filename: Option<&Path>, ) -> FrankaResult { - Ok(Model { + Ok(FR3Model { library: ModelLibrary::new(model_filename.as_ref(), libm_filename)?, }) } @@ -117,7 +511,7 @@ impl Model { /// * `EE_T_K` - Stiffness frame K in the end effector frame. /// # Return /// Vectorized 4x4 pose matrix, column-major. - pub fn pose( + fn pose( &self, frame: &Frame, q: &[f64; 7], @@ -155,7 +549,7 @@ impl Model { /// * `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: &PandaState) -> [f64; 16] { + fn pose_from_state(&self, frame: &Frame, robot_state: &Self::State) -> [f64; 16] { self.pose( frame, &robot_state.q, @@ -173,7 +567,7 @@ impl Model { /// * `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], @@ -213,7 +607,7 @@ impl Model { /// * `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: &PandaState) -> [f64; 42] { + fn body_jacobian_from_state(&self, frame: &Frame, robot_state: &Self::State) -> [f64; 42] { self.body_jacobian( frame, &robot_state.q, @@ -231,7 +625,7 @@ impl Model { /// * `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], @@ -270,7 +664,7 @@ impl Model { /// * `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: &PandaState) -> [f64; 42] { + fn zero_jacobian_from_state(&self, frame: &Frame, robot_state: &Self::State) -> [f64; 42] { self.zero_jacobian( frame, &robot_state.q, @@ -287,7 +681,7 @@ impl Model { /// * `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], @@ -304,7 +698,7 @@ impl Model { /// * `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: &PandaState) -> [f64; 49] { + fn mass_from_state(&self, robot_state: &Self::State) -> [f64; 49] { self.mass( &robot_state.q, &robot_state.I_total, @@ -323,7 +717,7 @@ impl Model { /// * `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], @@ -343,7 +737,7 @@ impl Model { /// * `robot_state` - State from which the Coriolis force vector should be calculated. /// # Return /// Coriolis force vector. - pub fn coriolis_from_state(&self, robot_state: &PandaState) -> [f64; 7] { + fn coriolis_from_state(&self, robot_state: &Self::State) -> [f64; 7] { self.coriolis( &robot_state.q, &robot_state.dq, @@ -361,7 +755,7 @@ impl Model { /// 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, @@ -381,9 +775,9 @@ impl Model { /// 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: &PandaState, + robot_state: &Self::State, gravity_earth: Grav, ) -> [f64; 7] { self.gravity( diff --git a/src/model/library_downloader.rs b/src/model/library_downloader.rs index 70f9c38..841c132 100644 --- a/src/model/library_downloader.rs +++ b/src/model/library_downloader.rs @@ -1,11 +1,8 @@ // Copyright (c) 2021 Marco Boneberger // Licensed under the EUPL-1.2-or-later use crate::exception::FrankaException::ModelException; -use crate::network::{FR3Data, Network, PandaData}; -use crate::robot::service_types::{ - LoadModelLibraryArchitecture, LoadModelLibraryRequest, LoadModelLibraryRequestWithHeader, - LoadModelLibraryResponse, LoadModelLibrarySystem, PandaCommandEnum, -}; +use crate::network::{DeviceData, FR3Data, Network, PandaData, RobotData}; +use crate::robot::service_types::{FR3CommandEnum, LoadModelLibraryArchitecture, LoadModelLibraryRequest, LoadModelLibraryRequestWithHeader, LoadModelLibraryResponse, LoadModelLibrarySystem, ModelRequestWithHeader, PandaCommandEnum, PandaCommandHeader, RobotHeader}; use crate::FrankaResult; use std::fmt; use std::fmt::Display; @@ -15,55 +12,24 @@ use std::io::Write; use std::mem::size_of; use std::path::Path; -pub struct LibraryDownloader {} -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( - PandaCommandEnum::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(()) - } else { - Err(ModelException { - message: - "Your platform is not yet supported for Downloading models. Please use Linux on\ - x86_64 for now" - .to_string(), - }) - } - } +pub trait LibraryDownloader{ + type Data:RobotData; + fn download(network: &mut Network, download_path: &Path) -> FrankaResult<()>; +} +pub struct LibraryDownloaderGeneric{ + data: std::marker::PhantomData +} +impl LibraryDownloader for LibraryDownloaderGeneric { + type Data = Data; - // TODO unify - pub fn download2(network: &mut Network, download_path: &Path) -> FrankaResult<()> { + 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_panda( - PandaCommandEnum::LoadModelLibrary, - size_of::(), - ), - request: LoadModelLibraryRequest { - architecture: LoadModelLibraryArchitecture::X64, - system: LoadModelLibrarySystem::Linux, - }, + let request= LoadModelLibraryRequest { + architecture: LoadModelLibraryArchitecture::X64, + system: LoadModelLibrarySystem::Linux, }; + 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 _response: LoadModelLibraryResponse = diff --git a/src/network.rs b/src/network.rs index 6b6ad86..5af25bc 100644 --- a/src/network.rs +++ b/src/network.rs @@ -27,11 +27,15 @@ use serde::de::DeserializeOwned; use serde::{Deserialize, Serialize}; use crate::exception::{FrankaException, FrankaResult}; -use crate::{gripper, GripperState, PandaState}; use crate::gripper::types::{CommandHeader, GripperCommandEnum, GripperCommandHeader}; use crate::robot::robot_state::{FR3State, RobotState}; -use crate::robot::service_types::{FR3CommandEnum, FR3CommandHeader, LoadModelLibraryResponse, LoadModelLibraryStatus, PandaCommandEnum, PandaCommandHeader}; -use crate::robot::types::{RobotStateIntern, PandaStateIntern}; +use crate::robot::service_types::{ + FR3CommandEnum, FR3CommandHeader, LoadModelLibraryRequest, LoadModelLibraryRequestWithHeader, + LoadModelLibraryResponse, LoadModelLibraryStatus, ModelRequestWithHeader, PandaCommandEnum, + PandaCommandHeader, RobotHeader, +}; +use crate::robot::types::{PandaStateIntern, RobotStateIntern}; +use crate::{gripper, FR3Model, GripperState, PandaModel, PandaState, RobotModel}; const CLIENT: Token = Token(1); @@ -44,37 +48,39 @@ pub enum NetworkType { pub trait DeviceData { type CommandHeader: CommandHeader; type CommandEnum; - fn create_header(command_id: &mut u32, - command: Self::CommandEnum, - size: usize, + fn create_header( + command_id: &mut u32, + command: Self::CommandEnum, + size: usize, ) -> Self::CommandHeader; } -pub trait RobotData : DeviceData { +pub trait RobotData: DeviceData { type DeviceData: DeviceData; - type State:RobotState + From; + type Header: RobotHeader; + type State: RobotState + From; type StateIntern: Debug + DeserializeOwned + Serialize + RobotStateIntern + 'static; - - - -} - -pub struct PandaData { - + type Model: RobotModel; + fn create_model_library_request( + command_id: &mut u32, + request: LoadModelLibraryRequest, + ) -> ModelRequestWithHeader<::Header>; } -pub struct FR3Data { - -} +pub struct PandaData {} -pub struct GripperData{ +pub struct FR3Data {} -} +pub struct GripperData {} impl DeviceData for PandaData { type CommandHeader = PandaCommandHeader; type CommandEnum = PandaCommandEnum; - fn create_header(command_id: &mut u32, command: Self::CommandEnum, size: usize) -> Self::CommandHeader { + 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 @@ -85,11 +91,30 @@ impl RobotData for PandaData { type State = PandaState; type DeviceData = Self; type StateIntern = PandaStateIntern; + type Model = PandaModel; + + fn create_model_library_request( + mut command_id: &mut u32, + request: LoadModelLibraryRequest, + ) -> ModelRequestWithHeader { + let header = Self::create_header( + &mut command_id, + PandaCommandEnum::LoadModelLibrary, + size_of::(), + ); + ModelRequestWithHeader { header, request } + } + + type Header = PandaCommandHeader; } impl DeviceData for FR3Data { type CommandHeader = FR3CommandHeader; type CommandEnum = FR3CommandEnum; - fn create_header(command_id: &mut u32, command: Self::CommandEnum, size: usize) -> Self::CommandHeader { + 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 @@ -97,15 +122,39 @@ impl DeviceData for FR3Data { } impl RobotData for FR3Data { type DeviceData = Self; + type Header = FR3CommandHeader; type State = FR3State; type StateIntern = PandaStateIntern; // todo create fr3stateintern type + type Model = FR3Model; + + fn create_model_library_request( + mut command_id: &mut u32, + request: LoadModelLibraryRequest, + ) -> ModelRequestWithHeader<<::DeviceData as DeviceData>::CommandHeader> + where + <::DeviceData as DeviceData>::CommandHeader: RobotHeader, + { + let header = Self::create_header( + &mut command_id, + FR3CommandEnum::LoadModelLibrary, + size_of::(), + ); + ModelRequestWithHeader { + header: header, + request: request, + } + } } impl DeviceData for GripperData { type CommandHeader = GripperCommandHeader; type CommandEnum = GripperCommandEnum; - fn create_header(command_id: &mut u32, command: Self::CommandEnum, size: usize) -> Self::CommandHeader { + 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 @@ -132,7 +181,7 @@ pub struct Network { events: Events, poll_read_udp: Poll, events_udp: Events, - data : PhantomData, + data: PhantomData, } impl Network { @@ -189,7 +238,7 @@ impl Network { events, poll_read_udp, events_udp, - data:PhantomData + data: PhantomData, }) } // pub fn create_header_for_gripper( @@ -216,8 +265,7 @@ impl Network { command: Data::CommandEnum, size: usize, ) -> Data::CommandHeader { - Data::create_header( &mut self.command_id, command, size ) - + Data::create_header(&mut self.command_id, command, size) } // pub fn create_header_for_fr3( // &mut self, @@ -229,10 +277,7 @@ impl Network { // header // } - 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() diff --git a/src/robot.rs b/src/robot.rs index 587c5bd..b2959c8 100644 --- a/src/robot.rs +++ b/src/robot.rs @@ -8,7 +8,7 @@ use std::time::Duration; use std::fmt::Debug; use crate::exception::{create_command_exception, FrankaException, FrankaResult}; -use crate::model::Model; +use crate::model::PandaModel; use crate::network::{FR3Data, Network, NetworkType, PandaData, RobotData}; use crate::robot::control_loop::ControlLoop; use crate::robot::control_types::{ @@ -18,7 +18,8 @@ use crate::robot::control_types::{ use crate::robot::low_pass_filter::{DEFAULT_CUTOFF_FREQUENCY, MAX_CUTOFF_FREQUENCY}; use crate::robot::motion_generator_traits::MotionGeneratorTrait; use crate::robot::robot_control::RobotControl; -use crate::robot::robot_impl::{RobotImplementation, RobotImplGeneric}; +use crate::robot::robot_impl::{RobotImplGeneric, RobotImplementation}; +use crate::robot::robot_state::{FR3State, RobotState}; use crate::robot::service_types::{ AutomaticErrorRecoveryRequestWithHeader, AutomaticErrorRecoveryResponse, AutomaticErrorRecoveryStatus, GetCartesianLimitRequest, GetCartesianLimitRequestWithHeader, @@ -34,9 +35,8 @@ use crate::robot::service_types::{ }; use crate::robot::virtual_wall_cuboid::VirtualWallCuboid; use crate::utils::MotionGenerator; -use robot_state::PandaState; use crate::Finishable; -use crate::robot::robot_state::{FR3State, RobotState}; +use robot_state::PandaState; mod control_loop; mod control_tools; @@ -54,25 +54,30 @@ pub(crate) mod service_types; pub(crate) mod types; pub mod virtual_wall_cuboid; -pub trait Robot where CartesianPose: ConvertMotion<::State1>{ - type Data:RobotData; - type State1:RobotState; - type Rob:RobotImplementation; +pub trait Robot +where + CartesianPose: ConvertMotion<<::Data as RobotData>::State>, + JointVelocities: ConvertMotion<<::Data as RobotData>::State>, + JointPositions: ConvertMotion<<::Data as RobotData>::State>, + CartesianVelocities: ConvertMotion<<::Data as RobotData>::State>, +{ + type Data: RobotData; + type Rob: RobotImplementation; fn get_rob(&mut self) -> &mut Self::Rob; fn get_net(&mut self) -> &mut Network; - fn read bool>(&mut self, - mut read_callback: F, + fn read::Data as RobotData>::State) -> bool>( + &mut self, + mut read_callback: F, ) -> FrankaResult<()> { loop { - let state = self.get_rob().update2(None, None)?; if !read_callback(&state) { - break; + break; } } Ok(()) } - fn read_once(&mut self) -> FrankaResult { + fn read_once(&mut self) -> FrankaResult<<::Data as RobotData>::State> { self.get_rob().read_once2() } @@ -105,9 +110,8 @@ pub trait Robot where CartesianPose: ConvertMotion<::State1>{ ), }; let command_id: u32 = self.get_net().tcp_send_request(command); - let response: SetCollisionBehaviorResponse = self - .get_net() - .tcp_blocking_receive_response(command_id); + let response: SetCollisionBehaviorResponse = + self.get_net().tcp_blocking_receive_response(command_id); handle_getter_setter_status(response.status) } @@ -121,9 +125,8 @@ pub trait Robot where CartesianPose: ConvertMotion<::State1>{ request: SetJointImpedanceRequest::new(K_theta), }; let command_id: u32 = self.get_net().tcp_send_request(command); - let response: SetJointImpedanceResponse = self - .get_net() - .tcp_blocking_receive_response(command_id); + let response: SetJointImpedanceResponse = + self.get_net().tcp_blocking_receive_response(command_id); handle_getter_setter_status(response.status) } @@ -137,15 +140,17 @@ pub trait Robot where CartesianPose: ConvertMotion<::State1>{ request: SetCartesianImpedanceRequest::new(K_x), }; let command_id: u32 = self.get_net().tcp_send_request(command); - let response: SetCartesianImpedanceResponse = self - .get_net() - .tcp_blocking_receive_response(command_id); + let response: SetCartesianImpedanceResponse = + self.get_net().tcp_blocking_receive_response(command_id); handle_getter_setter_status(response.status) } fn control_motion_intern< - F: FnMut(&Self::State1, &Duration) -> U, - U: ConvertMotion + Debug + MotionGeneratorTrait + Finishable, + F: FnMut(&<::Data as RobotData>::State, &Duration) -> U, + U: ConvertMotion<<::Data as RobotData>::State> + + Debug + + MotionGeneratorTrait + + Finishable, >( &mut self, motion_generator_callback: F, @@ -157,7 +162,7 @@ pub trait Robot where CartesianPose: ConvertMotion<::State1>{ 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( - self.get_rob(), + self.get_rob(), controller_mode, motion_generator_callback, limit_rate, @@ -167,7 +172,7 @@ pub trait Robot where CartesianPose: ConvertMotion<::State1>{ } fn control_cartesian_pose< - F: FnMut(&Self::State1, &Duration) -> CartesianPose, + F: FnMut(&<::Data as RobotData>::State, &Duration) -> CartesianPose, CM: Into>, L: Into>, CF: Into>, @@ -185,6 +190,269 @@ pub trait Robot where CartesianPose: ConvertMotion<::State1>{ cutoff_frequency.into(), ) } + + fn control_torques_intern< + F: FnMut(&<::Data as RobotData>::State, &Duration) -> U, + U: ConvertMotion<<::Data as RobotData>::State> + + Debug + + MotionGeneratorTrait + + Finishable, + >( + &mut self, + motion_generator_callback: F, + control_callback: &mut dyn FnMut( + &<::Data as RobotData>::State, + &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( + self.get_rob(), + control_callback, + motion_generator_callback, + limit_rate, + cutoff_frequency, + )?; + control_loop.run() + } + + /// 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. + fn control_torques< + T: FnMut(&<::Data as RobotData>::State, &Duration) -> Torques, + L: Into>, + CF: Into>, + >( + &mut self, + mut control_callback: T, + limit_rate: L, + cutoff_frequency: CF, + ) -> FrankaResult<()> { + let motion_generator_callback = + |_state: &<::Data as RobotData>::State, _time_step: &Duration| { + JointVelocities::new([0.; 7]) + }; + self.control_torques_intern( + &motion_generator_callback, + &mut control_callback, + 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. + fn control_torques_and_joint_velocities< + F: FnMut(&<::Data as RobotData>::State, &Duration) -> JointVelocities, + T: FnMut(&<::Data as RobotData>::State, &Duration) -> Torques, + L: Into>, + CF: Into>, + >( + &mut self, + mut control_callback: T, + motion_generator_callback: F, + limit_rate: L, + cutoff_frequency: CF, + ) -> FrankaResult<()> { + self.control_torques_intern( + motion_generator_callback, + &mut control_callback, + 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. + fn control_torques_and_joint_positions< + F: FnMut(&<::Data as RobotData>::State, &Duration) -> JointPositions, + T: FnMut(&<::Data as RobotData>::State, &Duration) -> Torques, + L: Into>, + CF: Into>, + >( + &mut self, + mut control_callback: T, + motion_generator_callback: F, + limit_rate: L, + cutoff_frequency: CF, + ) -> FrankaResult<()> { + self.control_torques_intern( + motion_generator_callback, + &mut control_callback, + 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. + fn control_torques_and_cartesian_pose< + F: FnMut(&<::Data as RobotData>::State, &Duration) -> CartesianPose, + T: FnMut(&<::Data as RobotData>::State, &Duration) -> Torques, + L: Into>, + CF: Into>, + >( + &mut self, + mut control_callback: T, + motion_generator_callback: F, + limit_rate: L, + cutoff_frequency: CF, + ) -> FrankaResult<()> { + self.control_torques_intern( + motion_generator_callback, + &mut control_callback, + 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. + fn control_torques_and_cartesian_velocities< + F: FnMut(&<::Data as RobotData>::State, &Duration) -> CartesianVelocities, + T: FnMut(&<::Data as RobotData>::State, &Duration) -> Torques, + L: Into>, + CF: Into>, + >( + &mut self, + mut control_callback: T, + motion_generator_callback: F, + limit_rate: L, + cutoff_frequency: CF, + ) -> FrankaResult<()> { + self.control_torques_intern( + motion_generator_callback, + &mut control_callback, + limit_rate.into(), + cutoff_frequency.into(), + ) + } + + /// 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. + fn load_model(&mut self, persistent: bool) -> FrankaResult<::Model> { + self.get_rob().load_model2(persistent) + } } /// Maintains a network connection to the robot, provides the current robot state, gives access to @@ -246,33 +514,31 @@ pub struct Panda { impl Robot for Panda { type Data = PandaData; - type State1 = PandaState; type Rob = RobotImplGeneric; - fn get_net(&mut self) -> &mut Network{ + fn get_net(&mut self) -> &mut Network { &mut self.robimpl.network } - fn get_rob(&mut self) ->&mut Self::Rob { + fn get_rob(&mut self) -> &mut Self::Rob { &mut self.robimpl } } pub struct FR3 { - robimpl: RobotImplGeneric, + robimpl: RobotImplGeneric, } impl Robot for FR3 { type Data = FR3Data; - type State1 = FR3State; type Rob = RobotImplGeneric; fn get_net(&mut self) -> &mut Network { &mut self.robimpl.network } - fn get_rob(&mut self) ->&mut Self::Rob { + fn get_rob(&mut self) -> &mut Self::Rob { &mut self.robimpl } } -impl FR3{ +impl FR3 { pub fn new>, LogSize: Into>>( franka_address: &str, realtime_config: RtConfig, @@ -280,13 +546,14 @@ impl FR3{ ) -> 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::FR3, + let network = Network::new( + NetworkType::FR3, franka_address, service_types::COMMAND_PORT, ) - .map_err(|_| FrankaException::NetworkException { - message: "Connection could not be established".to_string(), - })?; + .map_err(|_| FrankaException::NetworkException { + message: "Connection could not be established".to_string(), + })?; Ok(FR3 { robimpl: ::Rob::new(network, log_size, realtime_config)?, }) @@ -323,7 +590,8 @@ impl Panda { ) -> 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::Panda, + let network = Network::new( + NetworkType::Panda, franka_address, service_types::COMMAND_PORT, ) @@ -940,268 +1208,7 @@ impl Panda { 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(&PandaState, &Duration) -> JointVelocities, - T: FnMut(&PandaState, &Duration) -> Torques, - L: Into>, - CF: Into>, - >( - &mut self, - mut control_callback: T, - motion_generator_callback: F, - limit_rate: L, - cutoff_frequency: CF, - ) -> FrankaResult<()> { - self.control_torques_intern( - motion_generator_callback, - &mut control_callback, - 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< - F: FnMut(&PandaState, &Duration) -> JointPositions, - T: FnMut(&PandaState, &Duration) -> Torques, - L: Into>, - CF: Into>, - >( - &mut self, - mut control_callback: T, - motion_generator_callback: F, - limit_rate: L, - cutoff_frequency: CF, - ) -> FrankaResult<()> { - self.control_torques_intern( - motion_generator_callback, - &mut control_callback, - 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(&PandaState, &Duration) -> CartesianPose, - T: FnMut(&PandaState, &Duration) -> Torques, - L: Into>, - CF: Into>, - >( - &mut self, - mut control_callback: T, - motion_generator_callback: F, - limit_rate: L, - cutoff_frequency: CF, - ) -> FrankaResult<()> { - self.control_torques_intern( - motion_generator_callback, - &mut control_callback, - 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(&PandaState, &Duration) -> CartesianVelocities, - T: FnMut(&PandaState, &Duration) -> Torques, - L: Into>, - CF: Into>, - >( - &mut self, - mut control_callback: T, - motion_generator_callback: F, - limit_rate: L, - cutoff_frequency: CF, - ) -> FrankaResult<()> { - self.control_torques_intern( - motion_generator_callback, - &mut control_callback, - 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< - T: FnMut(&PandaState, &Duration) -> Torques, - L: Into>, - CF: Into>, - >( - &mut self, - mut control_callback: T, - limit_rate: L, - cutoff_frequency: CF, - ) -> FrankaResult<()> { - let motion_generator_callback = - |_state: &PandaState, _time_step: &Duration| JointVelocities::new([0.; 7]); - self.control_torques_intern( - &motion_generator_callback, - &mut control_callback, - limit_rate.into(), - cutoff_frequency.into(), - ) - } - fn control_torques_intern< - F: FnMut(&PandaState, &Duration) -> U, - U: ConvertMotion + Debug + MotionGeneratorTrait + Finishable, - >( - &mut self, - motion_generator_callback: F, - control_callback: &mut dyn FnMut(&PandaState, &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 control_motion_intern< - // F: FnMut(&PandaState, &Duration) -> U, - // U: Finishable + Debug + MotionGeneratorTrait, - // >( - // &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(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() - // } /// Sets a default collision behavior, joint impedance and Cartesian impedance. /// # Errors /// * [`CommandException`](`crate::exception::FrankaException::CommandException`) if the Control reports an error. @@ -1229,18 +1236,6 @@ impl Panda { Some(MAX_CUTOFF_FREQUENCY), ) } - /// 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) // todo care about model - FrankaResult::Err(FrankaException::ModelException { message: "fuck".to_string() }) - } /// Returns the software version reported by the connected server. /// /// # Return @@ -1280,12 +1275,12 @@ mod tests { SetCollisionBehaviorResponse, COMMAND_PORT, VERSION, }; use crate::robot::types::PandaStateIntern; - use crate::{FrankaResult, JointPositions, RealtimeConfig, Panda, PandaState, Finishable}; + use crate::robot::Robot; + use crate::{Finishable, FrankaResult, JointPositions, Panda, PandaState, RealtimeConfig}; use bincode::{deserialize, serialize, serialized_size}; use std::iter::FromIterator; use std::mem::size_of; use std::time::{Duration, Instant}; - use crate::robot::Robot; struct Socket), G: Fn(&mut Vec)> { pub send_bytes: F, diff --git a/src/robot/control_loop.rs b/src/robot/control_loop.rs index 11ff371..defbe77 100644 --- a/src/robot/control_loop.rs +++ b/src/robot/control_loop.rs @@ -4,7 +4,7 @@ use std::fmt::Debug; use std::time::Duration; use crate::exception::{FrankaException, FrankaResult}; -use crate::Finishable; +use crate::network::RobotData; use crate::robot::control_tools::{ has_realtime_kernel, set_current_thread_to_highest_scheduler_priority, }; @@ -17,19 +17,24 @@ use crate::robot::robot_impl::RobotImplementation; use crate::robot::robot_state::{PandaState, RobotState}; use crate::robot::service_types::{MoveControllerMode, MoveDeviation}; use crate::robot::types::{ControllerCommand, MotionGeneratorCommand}; +use crate::Finishable; -type ControlCallback<'b,State> = &'b mut dyn FnMut(&State, &Duration) -> Torques; +type ControlCallback<'b, State> = &'b mut dyn FnMut(&State, &Duration) -> Torques; pub struct ControlLoop< 'a, 'b, T: RobotImplementation, - U: ConvertMotion + Debug + MotionGeneratorTrait + Finishable, - F: FnMut(&T::State, &Duration) -> U, + U: ConvertMotion<<::Data as RobotData>::State> + + Debug + + MotionGeneratorTrait + + Finishable, + F: FnMut(&<::Data as RobotData>::State, &Duration) -> U, > { pub default_deviation: MoveDeviation, robot: &'a mut T, motion_callback: F, - control_callback: Option>, + control_callback: + Option::Data as RobotData>::State>>, limit_rate: bool, cutoff_frequency: f64, pub motion_id: u32, @@ -39,13 +44,19 @@ impl< 'a, 'b, T: RobotImplementation, - U: ConvertMotion + Debug + MotionGeneratorTrait + Finishable, - F: FnMut(&T::State, &Duration) -> U, + U: ConvertMotion<<::Data as RobotData>::State> + + Debug + + MotionGeneratorTrait + + Finishable, + F: FnMut(&<::Data as RobotData>::State, &Duration) -> U, > ControlLoop<'a, 'b, T, U, F> { pub fn new( robot: &'a mut T, - control_callback: ControlCallback<'b,T::State>, + control_callback: ControlCallback< + 'b, + <::Data as RobotData>::State, + >, motion_callback: F, limit_rate: bool, cutoff_frequency: f64, @@ -89,7 +100,9 @@ impl< fn new_intern( robot: &'a mut T, motion_callback: F, - control_callback: Option>, + control_callback: Option< + ControlCallback<'b, <::Data as RobotData>::State>, + >, limit_rate: bool, cutoff_frequency: f64, ) -> FrankaResult { @@ -179,7 +192,7 @@ impl< fn spin_control( &mut self, - robot_state: &T::State, + robot_state: &<::Data as RobotData>::State, time_step: &Duration, command: &mut ControllerCommand, ) -> bool { diff --git a/src/robot/robot_impl.rs b/src/robot/robot_impl.rs index b4998e8..93018ab 100644 --- a/src/robot/robot_impl.rs +++ b/src/robot/robot_impl.rs @@ -3,8 +3,8 @@ 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::model::library_downloader::{LibraryDownloader, LibraryDownloaderGeneric}; +use crate::model::PandaModel; use crate::network::DeviceData; use crate::network::{FR3Data, Network, PandaData, RobotData}; use crate::robot::control_types::RealtimeConfig; @@ -22,14 +22,15 @@ use crate::robot::service_types::{ use crate::robot::types::{ControllerCommand, ControllerMode, RobotStateIntern, MotionGeneratorCommand, MotionGeneratorMode, PandaStateIntern, RobotCommand, RobotMode}; use std::fs::remove_file; use std::path::Path; +use crate::RobotModel; -pub trait RobotImplementation: RobotControl { - type State: RobotState; +pub trait RobotImplementation: RobotControl::Data as RobotData>::State> { + type Data: RobotData; fn update2( &mut self, motion_command: Option<&MotionGeneratorCommand>, control_command: Option<&ControllerCommand>, - ) -> FrankaResult; // todo remove commented out code + ) -> FrankaResult<<::Data as RobotData>::State>; // todo remove commented out code // { // let robot_command = self.send_robot_command(motion_command, control_command)?; // let state = Self::State::from(self.receive_robot_state()?); @@ -38,13 +39,13 @@ pub trait RobotImplementation: RobotControl { // } // Ok(state) // } - fn read_once2(&mut self) -> FrankaResult; - fn load_model2(&mut self, persistent: bool) -> FrankaResult; + fn read_once2(&mut self) -> FrankaResult<<::Data as RobotData>::State>; + fn load_model2(&mut self, persistent: bool) -> FrankaResult<<::Data as RobotData>::Model>; fn server_version2(&self) -> u16; } pub struct RobotImplGeneric { - pub network: Network<(Data::DeviceData)>, + pub network: Network, logger: Logger, realtime_config: RealtimeConfig, ri_version: Option, @@ -229,10 +230,10 @@ impl RobotControl for RobotImplGeneric { } } -impl RobotImplementation for RobotImplGeneric { - type State = Data::State; +impl> RobotImplementation for RobotImplGeneric { + type Data = Data; - fn update2(&mut self, motion_command: Option<&MotionGeneratorCommand>, control_command: Option<&ControllerCommand>) -> FrankaResult { + fn update2(&mut self, motion_command: Option<&MotionGeneratorCommand>, control_command: Option<&ControllerCommand>) -> FrankaResult<<::Data as RobotData>::State> { let robot_command = self.send_robot_command(motion_command, control_command)?; let state = Data::State::from(self.receive_robot_state()?); if let Some(command) = robot_command { @@ -241,24 +242,24 @@ impl RobotImplementation for RobotImplGeneric { Ok(state) } - fn read_once2(&mut self) -> FrankaResult { + fn read_once2(&mut self) -> FrankaResult<<::Data as RobotData>::State> { while self.network.udp_receive::().is_some() {} Ok(Data::State::from(self.receive_robot_state()?)) } - fn load_model2(&mut self, persistent: bool) -> FrankaResult { - /// todo take care of model - // 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) - FrankaResult::Err(FrankaException::ModelException { message: "fuck".to_string() }) + fn load_model2(&mut self, persistent: bool) -> FrankaResult{ + let model_file = Path::new("/tmp/model.so"); + 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_version2(&self) -> u16 { self.ri_version.unwrap() } @@ -461,19 +462,6 @@ impl RobotImplGeneric { fn controller_running(&self) -> bool { self.controller_mode == ControllerMode::ExternalController } - /// todo take care of model - // 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() } diff --git a/src/robot/service_types.rs b/src/robot/service_types.rs index d731278..ff5c491 100644 --- a/src/robot/service_types.rs +++ b/src/robot/service_types.rs @@ -639,6 +639,20 @@ pub struct LoadModelLibraryRequestWithHeader { pub request: LoadModelLibraryRequest, } +// this is a very hacky generic struct that is a generic version of LoadModelLibraryRequestWithHeader +// todo find a better solution to deal with the model downloader +#[derive(Serialize, Debug, Copy, Clone)] +pub struct ModelRequestWithHeader { + pub header: Header, + pub request: LoadModelLibraryRequest, +} + +impl MessageCommand for ModelRequestWithHeader
{ + fn get_command_message_id(&self) -> u32 { + self.header.get_command_message_id() + } +} + impl MessageCommand for LoadModelLibraryRequestWithHeader { fn get_command_message_id(&self) -> u32 { self.header.get_command_message_id() From 997c782279740b2d339fdd58202079e01bebe32c Mon Sep 17 00:00:00 2001 From: Marco Boneberger Date: Wed, 21 Dec 2022 13:42:56 +0100 Subject: [PATCH 06/63] implement requestwithheaders for panda and fr3 --- Cargo.toml | 2 +- examples/cartesian_impedance_control.rs | 6 +- examples/communication_test.rs | 12 +- examples/download_model.rs | 4 +- examples/echo_robot_state.rs | 10 +- examples/generate_cartesian_pose_motion.rs | 6 +- .../generate_cartesian_velocity_motion.rs | 8 +- examples/generate_consecutive_motions.rs | 5 +- examples/generate_elbow_motion.rs | 5 +- examples/generate_joint_position_motion.rs | 5 +- examples/generate_joint_velocity_motion.rs | 8 +- examples/mirror_robot.rs | 8 +- examples/print_joint_poses.rs | 5 +- src/exception.rs | 2 +- src/gripper.rs | 98 +- src/gripper/types.rs | 21 +- src/lib.rs | 2 +- src/model.rs | 16 +- src/model/library_downloader.rs | 25 +- src/network.rs | 631 ++++++++- src/robot.rs | 1209 +++++++---------- src/robot/control_types.rs | 414 +++--- src/robot/logger.rs | 6 +- src/robot/robot_control.rs | 2 +- src/robot/robot_impl.rs | 260 ++-- src/robot/robot_state.rs | 393 +----- src/robot/service_types.rs | 532 +++++--- src/robot/types.rs | 10 +- src/utils.rs | 10 +- 29 files changed, 1919 insertions(+), 1796 deletions(-) diff --git a/Cargo.toml b/Cargo.toml index 98048fd..b410ec4 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -68,7 +68,7 @@ name = "print_joint_poses" path = "examples/print_joint_poses.rs" [profile.dev] -opt-level = 3 +opt-level = 0 [dependencies] serde = { version = "1.0", features = ["derive"] } diff --git a/examples/cartesian_impedance_control.rs b/examples/cartesian_impedance_control.rs index 043b6ff..f1765df 100644 --- a/examples/cartesian_impedance_control.rs +++ b/examples/cartesian_impedance_control.rs @@ -2,14 +2,14 @@ // Licensed under the EUPL-1.2-or-later use clap::Parser; -use franka::{Frame, Panda, PandaState, RobotModel}; +use franka::robot::robot_state::FR3State; +use franka::robot::{Robot, FR3}; use franka::FrankaResult; use franka::Torques; use franka::{array_to_isometry, Matrix6x7, Vector7}; +use franka::{Frame, RobotModel}; use nalgebra::{Matrix3, Matrix6, Matrix6x1, UnitQuaternion, Vector3, U1, U3}; use std::time::Duration; -use franka::robot::{FR3, Robot}; -use franka::robot::robot_state::FR3State; /// An example showing a simple cartesian impedance controller without inertia shaping /// that renders a spring damper system where the equilibrium is the initial configuration. diff --git a/examples/communication_test.rs b/examples/communication_test.rs index e3e953a..ad82744 100644 --- a/examples/communication_test.rs +++ b/examples/communication_test.rs @@ -1,10 +1,10 @@ // 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 franka::robot::robot_state::FR3State; +use franka::robot::{Robot, FR3}; +use franka::Torques; +use franka::{Finishable, FrankaResult, PandaState}; use std::f64::consts::PI; use std::time::Duration; @@ -20,7 +20,7 @@ struct CommandLineArguments { fn main() -> FrankaResult<()> { let args = CommandLineArguments::parse(); - let mut robot = Robot::new(args.franka_ip.as_str(), None, None)?; + let mut robot = FR3::new(args.franka_ip.as_str(), None, None)?; 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!"); @@ -47,7 +47,7 @@ fn main() -> FrankaResult<()> { let mut min_success_rate = 1.; let mut max_success_rate = 0.; - let callback = |state: &RobotState, time_step: &Duration| -> Torques { + let callback = |state: &PandaState, time_step: &Duration| -> Torques { time += time_step.as_millis() as u64; if time == 0 { return zero_torques; diff --git a/examples/download_model.rs b/examples/download_model.rs index 745b1d1..f63e40f 100644 --- a/examples/download_model.rs +++ b/examples/download_model.rs @@ -3,7 +3,7 @@ use clap::Parser; use franka::exception::FrankaException::ModelException; -use franka::Robot; +use franka::robot::{Robot, FR3}; use franka::{FrankaResult, RealtimeConfig}; use std::fs; use std::path::PathBuf; @@ -24,7 +24,7 @@ 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)?; + let mut robot = FR3::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(), diff --git a/examples/echo_robot_state.rs b/examples/echo_robot_state.rs index 3e54c13..de54df0 100644 --- a/examples/echo_robot_state.rs +++ b/examples/echo_robot_state.rs @@ -2,8 +2,8 @@ // Licensed under the EUPL-1.2-or-later use clap::Parser; +use franka::robot::{Robot, FR3}; use franka::{FrankaResult, Panda, PandaState}; -use franka::robot::Robot; // use franka::Robot; // use franka::RobotState; @@ -16,8 +16,10 @@ struct CommandLineArguments { } fn main() -> FrankaResult<()> { - let address = CommandLineArguments::parse(); - let mut robot = Panda::new(address.franka_ip.as_str(), None, None)?; + // let address = CommandLineArguments::parse(); + let mut robot = FR3::new("localhost", None, None)?; + // robot.set_collision_behavior([0.;7],[0.;7],[0.;7],[0.;7],[0.;6],[0.;6],[0.;6],[0.;6]); + robot.set_joint_impedance([3000., 3000., 3000., 2500., 2500., 2000., 2000.])?; let mut count = 0; robot.read(|robot_state: &PandaState| { // Printing to standard output adds a delay. This is acceptable for a read loop such as this, but @@ -26,6 +28,6 @@ fn main() -> FrankaResult<()> { count += 1; count <= 100 })?; - println!("Done"); + // println!("Done"); Ok(()) } diff --git a/examples/generate_cartesian_pose_motion.rs b/examples/generate_cartesian_pose_motion.rs index 1e25174..203b8a1 100644 --- a/examples/generate_cartesian_pose_motion.rs +++ b/examples/generate_cartesian_pose_motion.rs @@ -4,12 +4,12 @@ use clap::Parser; use franka::{ConvertMotion, Finishable, FrankaResult, Panda}; // use franka::Robot; +use franka::robot::robot_state::FR3State; +use franka::robot::{Robot, FR3}; +use franka::CartesianPose; use franka::PandaState; -use franka::{CartesianPose}; use std::f64::consts::PI; use std::time::Duration; -use franka::robot::{FR3, Robot}; -use franka::robot::robot_state::FR3State; /// An example showing how to generate a Cartesian motion. /// diff --git a/examples/generate_cartesian_velocity_motion.rs b/examples/generate_cartesian_velocity_motion.rs index 56a15e3..bbe50b6 100644 --- a/examples/generate_cartesian_velocity_motion.rs +++ b/examples/generate_cartesian_velocity_motion.rs @@ -2,7 +2,9 @@ // Licensed under the EUPL-1.2-or-later use clap::Parser; -use franka::{CartesianVelocities, FrankaResult, MotionFinished, Robot, RobotState}; +use franka::robot::robot_state::FR3State; +use franka::robot::{Robot, FR3}; +use franka::{CartesianVelocities, Finishable, FrankaResult}; use std::f64::consts::PI; use std::time::Duration; @@ -18,7 +20,7 @@ struct CommandLineArguments { fn main() -> FrankaResult<()> { let address = CommandLineArguments::parse(); - let mut robot = Robot::new(address.franka_ip.as_str(), None, None)?; + let mut robot = FR3::new(address.franka_ip.as_str(), 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..."); @@ -54,7 +56,7 @@ fn main() -> FrankaResult<()> { let v_max = 0.1; let angle = PI / 4.; let mut time = 0.; - let callback = |_state: &RobotState, period: &Duration| -> CartesianVelocities { + let callback = |_state: &FR3State, period: &Duration| -> CartesianVelocities { time += period.as_secs_f64(); let cycle = f64::floor(f64::powf( diff --git a/examples/generate_consecutive_motions.rs b/examples/generate_consecutive_motions.rs index 64022d9..0eb6721 100644 --- a/examples/generate_consecutive_motions.rs +++ b/examples/generate_consecutive_motions.rs @@ -3,7 +3,8 @@ use clap::Parser; use franka::exception::FrankaException; -use franka::{FrankaResult, JointVelocities, MotionFinished, Robot, PandaState}; +use franka::robot::{Robot, FR3}; +use franka::{Finishable, FrankaResult, JointVelocities, PandaState}; use std::f64::consts::PI; use std::time::Duration; @@ -20,7 +21,7 @@ struct CommandLineArguments { fn main() -> FrankaResult<()> { let address = CommandLineArguments::parse(); - let mut robot = Robot::new(address.franka_ip.as_str(), None, None)?; + let mut robot = FR3::new(address.franka_ip.as_str(), 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..."); diff --git a/examples/generate_elbow_motion.rs b/examples/generate_elbow_motion.rs index 8f7f84e..32158e3 100644 --- a/examples/generate_elbow_motion.rs +++ b/examples/generate_elbow_motion.rs @@ -2,7 +2,8 @@ // Licensed under the EUPL-1.2-or-later use clap::Parser; -use franka::{CartesianPose, FrankaResult, MotionFinished, Robot, PandaState}; +use franka::robot::{Robot, FR3}; +use franka::{CartesianPose, Finishable, FrankaResult, PandaState}; use std::f64::consts::PI; use std::time::Duration; @@ -18,7 +19,7 @@ struct CommandLineArguments { fn main() -> FrankaResult<()> { let address = CommandLineArguments::parse(); - let mut robot = Robot::new(address.franka_ip.as_str(), None, None)?; + let mut robot = FR3::new(address.franka_ip.as_str(), 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..."); diff --git a/examples/generate_joint_position_motion.rs b/examples/generate_joint_position_motion.rs index 6ab8d2a..3ae36c9 100644 --- a/examples/generate_joint_position_motion.rs +++ b/examples/generate_joint_position_motion.rs @@ -2,7 +2,8 @@ // Licensed under the EUPL-1.2-or-later use clap::Parser; -use franka::{FrankaResult, JointPositions, MotionFinished, Robot, PandaState}; +use franka::robot::{Robot, FR3}; +use franka::{Finishable, FrankaResult, JointPositions, PandaState}; use std::f64::consts::PI; use std::time::Duration; @@ -18,7 +19,7 @@ struct CommandLineArguments { fn main() -> FrankaResult<()> { let address = CommandLineArguments::parse(); - let mut robot = Robot::new(address.franka_ip.as_str(), None, None)?; + let mut robot = FR3::new(address.franka_ip.as_str(), 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..."); diff --git a/examples/generate_joint_velocity_motion.rs b/examples/generate_joint_velocity_motion.rs index ba81a40..3ed34af 100644 --- a/examples/generate_joint_velocity_motion.rs +++ b/examples/generate_joint_velocity_motion.rs @@ -2,10 +2,10 @@ // Licensed under the EUPL-1.2-or-later use clap::Parser; -use franka::FrankaResult; -use franka::Robot; +use franka::robot::{Robot, FR3}; +use franka::JointVelocities; use franka::PandaState; -use franka::{JointVelocities, MotionFinished}; +use franka::{Finishable, FrankaResult}; use std::f64::consts::PI; use std::time::Duration; @@ -21,7 +21,7 @@ struct CommandLineArguments { fn main() -> FrankaResult<()> { let address = CommandLineArguments::parse(); - let mut robot = Robot::new(address.franka_ip.as_str(), None, None)?; + let mut robot = FR3::new(address.franka_ip.as_str(), 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..."); diff --git a/examples/mirror_robot.rs b/examples/mirror_robot.rs index 9210889..047209a 100644 --- a/examples/mirror_robot.rs +++ b/examples/mirror_robot.rs @@ -5,10 +5,10 @@ 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::robot::{Robot, FR3}; use franka::utils::{array_to_isometry, Matrix6x7, Vector7}; -use franka::Matrix7; use franka::PandaState; +use franka::{Matrix7, RobotModel}; use nalgebra::{Matrix3, Matrix6, Matrix6x1, Quaternion, UnitQuaternion, Vector3, U1, U3}; use std::sync::mpsc::channel; use std::time::Duration; @@ -50,8 +50,8 @@ fn main() -> FrankaResult<()> { 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 mut robot_user = FR3::new(args.franka_ip_user.as_str(), None, None)?; + let mut robot_mirror = FR3::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], diff --git a/examples/print_joint_poses.rs b/examples/print_joint_poses.rs index c122cce..986ca0b 100644 --- a/examples/print_joint_poses.rs +++ b/examples/print_joint_poses.rs @@ -1,7 +1,8 @@ // Copyright (c) 2021 Marco Boneberger // Licensed under the EUPL-1.2-or-later use clap::Parser; -use franka::{Frame, FrankaResult, RealtimeConfig, Robot}; +use franka::robot::{Robot, FR3}; +use franka::{Frame, FrankaResult, RealtimeConfig, RobotModel}; use nalgebra::Matrix4; /// An example showing how to use the model library that prints the transformation @@ -15,7 +16,7 @@ struct CommandLineArguments { fn main() -> FrankaResult<()> { let args = CommandLineArguments::parse(); - let mut robot = Robot::new(args.franka_ip.as_str(), RealtimeConfig::Ignore, None)?; + let mut robot = FR3::new(args.franka_ip.as_str(), RealtimeConfig::Ignore, None)?; let model = robot.load_model(false)?; let state = robot.read_once()?; let frames = vec![ diff --git a/src/exception.rs b/src/exception.rs index 60c8b95..e3b9bd9 100644 --- a/src/exception.rs +++ b/src/exception.rs @@ -3,8 +3,8 @@ //! Contains exception and Result definitions use crate::robot::logger::Record; -use thiserror::Error; use crate::PandaState; +use thiserror::Error; /// Represents all kind of errors which correspond to the franka::Exception in the C++ version of /// this library diff --git a/src/gripper.rs b/src/gripper.rs index 2bcef66..8cf12c9 100644 --- a/src/gripper.rs +++ b/src/gripper.rs @@ -8,10 +8,9 @@ use std::mem::size_of; use crate::exception::{create_command_exception, FrankaException, FrankaResult}; use crate::gripper::gripper_state::GripperState; use crate::gripper::types::{ - GripperCommandEnum, 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, VERSION, }; use crate::network::{GripperData, Network, NetworkType}; @@ -46,9 +45,10 @@ impl Gripper { } fn connect_gripper(&mut self, ri_version: &u16) -> FrankaResult<()> { let connect_command = ConnectRequestWithHeader { - header: self - .network - .create_header(GripperCommandEnum::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); @@ -82,8 +82,8 @@ impl Gripper { 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 +96,13 @@ impl Gripper { /// # Return /// True if command was successful, false otherwise. pub fn homing(&mut self) -> FrankaResult { - let command: HomingRequestWithHeader = self - .network - .create_header(GripperCommandEnum::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 +112,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(GripperCommandEnum::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 +148,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(GripperCommandEnum::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 @@ -194,10 +196,9 @@ fn handle_response_status(status: &Status) -> FrankaResult { #[cfg(test)] mod tests { use crate::gripper::types::{ - GripperCommandEnum, GripperCommandHeader, ConnectRequestWithHeader, ConnectResponse, GraspRequest, - GraspRequestWithHeader, GraspResponse, GripperStateIntern, HomingRequestWithHeader, - HomingResponse, MoveRequest, MoveRequestWithHeader, MoveResponse, Status, - StopRequestWithHeader, StopResponse, COMMAND_PORT, VERSION, + ConnectRequestWithHeader, ConnectResponse, GraspRequest, GraspRequestWithHeader, + GripperCommandEnum, GripperCommandHeader, GripperStateIntern, MoveRequest, + MoveRequestWithHeader, Status, COMMAND_PORT, VERSION, }; use crate::gripper::Gripper; use crate::FrankaResult; @@ -214,9 +215,16 @@ mod tests { use crate::exception::FrankaException; use crate::gripper::types::Status::{Fail, Success}; use crate::network::MessageCommand; + use serde::{Deserialize, Serialize}; use std::iter::FromIterator; use std::mem::size_of; + #[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, @@ -415,8 +423,12 @@ mod tests { .for_each(|(x, y)| assert_eq!(x, y)); let req: MoveRequestWithHeader = deserialize(&bytes).unwrap(); counter += 1; - let mut response = MoveResponse { - header: GripperCommandHeader::new(GripperCommandEnum::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; @@ -445,15 +457,19 @@ mod tests { 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 { GripperCommandEnum::Stop => {} _ => { assert!(false) } } - let mut response = StopResponse { - header: GripperCommandHeader::new(GripperCommandEnum::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; @@ -479,15 +495,19 @@ mod tests { 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 { GripperCommandEnum::Homing => {} _ => { assert!(false) } } - let mut response = HomingResponse { - header: GripperCommandHeader::new(GripperCommandEnum::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; @@ -548,8 +568,12 @@ mod tests { .for_each(|(x, y)| assert_eq!(x, y)); let req: MoveRequestWithHeader = deserialize(&bytes).unwrap(); counter += 1; - let mut response = GraspResponse { - header: GripperCommandHeader::new(GripperCommandEnum::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; diff --git a/src/gripper/types.rs b/src/gripper/types.rs index a203c54..4510fa6 100644 --- a/src/gripper/types.rs +++ b/src/gripper/types.rs @@ -8,8 +8,8 @@ use serde::Serialize; use serde_repr::{Deserialize_repr, Serialize_repr}; use crate::network::MessageCommand; -use std::time::Duration; use serde::de::DeserializeOwned; +use std::time::Duration; pub static VERSION: u16 = 3; pub static COMMAND_PORT: u16 = 1338; @@ -24,7 +24,7 @@ pub enum GripperCommandEnum { Stop, } -#[derive(Serialize_repr, Deserialize_repr, Debug)] +#[derive(Serialize_repr, Deserialize_repr, Debug, Copy, Clone)] #[repr(u16)] pub enum Status { Success, @@ -49,7 +49,7 @@ impl GripperStateIntern { } } // TODO is static a problem? -pub trait CommandHeader : Debug + DeserializeOwned + 'static { +pub trait CommandHeader: Serialize + MessageCommand + Debug + DeserializeOwned + 'static { fn get_command_id(&self) -> u32; fn get_size(&self) -> u32; } @@ -68,7 +68,7 @@ impl CommandHeader for GripperCommandHeader { } fn get_size(&self) -> u32 { - self.size + self.size } } @@ -154,9 +154,6 @@ pub struct MoveRequestWithHeader { pub request: MoveRequest, } -pub type HomingRequestWithHeader = GripperCommandHeader; -pub type StopRequestWithHeader = GripperCommandHeader; - #[derive(Serialize, Deserialize, Debug, Copy, Clone)] #[repr(packed)] pub struct GraspRequestWithHeader { @@ -194,13 +191,3 @@ pub struct ConnectResponse { pub status: Status, pub version: u16, } - -#[derive(Serialize, Deserialize, Debug)] -pub struct MoveResponse { - pub header: GripperCommandHeader, - 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 5097126..4254954 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -174,10 +174,10 @@ pub mod utils; pub use exception::FrankaResult; pub use gripper::gripper_state::GripperState; pub use gripper::Gripper; +pub use model::FR3Model; pub use model::Frame; pub use model::PandaModel; pub use model::RobotModel; -pub use model::FR3Model; pub use robot::control_types::*; pub use robot::low_pass_filter::DEFAULT_CUTOFF_FREQUENCY; pub use robot::low_pass_filter::MAX_CUTOFF_FREQUENCY; diff --git a/src/model.rs b/src/model.rs index d91a467..0f55177 100644 --- a/src/model.rs +++ b/src/model.rs @@ -66,7 +66,9 @@ impl fmt::Display for Frame { pub trait RobotModel { type State: RobotState; - fn new>(model_filename: S, libm_filename: Option<&Path>) -> FrankaResult where Self: Sized; + fn new>(model_filename: S, libm_filename: Option<&Path>) -> FrankaResult + where + Self: Sized; fn pose( &self, @@ -161,10 +163,7 @@ impl RobotModel for PandaModel { /// 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 \ ¯\_(ツ)_/¯ - fn new>( - model_filename: S, - libm_filename: Option<&Path>, - ) -> FrankaResult { + fn new>(model_filename: S, libm_filename: Option<&Path>) -> FrankaResult { Ok(PandaModel { library: ModelLibrary::new(model_filename.as_ref(), libm_filename)?, }) @@ -385,7 +384,7 @@ impl RobotModel for PandaModel { /// * `F_x_Ctotal` - Translation from flange to center of mass of the attached total load. Unit: \[m\]. /// # Return /// Coriolis force vector. - fn coriolis( + fn coriolis( &self, q: &[f64; 7], dq: &[f64; 7], @@ -493,10 +492,7 @@ impl RobotModel for FR3Model { /// 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 \ ¯\_(ツ)_/¯ - fn new>( - model_filename: S, - libm_filename: Option<&Path>, - ) -> FrankaResult { + fn new>(model_filename: S, libm_filename: Option<&Path>) -> FrankaResult { Ok(FR3Model { library: ModelLibrary::new(model_filename.as_ref(), libm_filename)?, }) diff --git a/src/model/library_downloader.rs b/src/model/library_downloader.rs index 841c132..03c20d8 100644 --- a/src/model/library_downloader.rs +++ b/src/model/library_downloader.rs @@ -2,7 +2,11 @@ // Licensed under the EUPL-1.2-or-later use crate::exception::FrankaException::ModelException; use crate::network::{DeviceData, FR3Data, Network, PandaData, RobotData}; -use crate::robot::service_types::{FR3CommandEnum, LoadModelLibraryArchitecture, LoadModelLibraryRequest, LoadModelLibraryRequestWithHeader, LoadModelLibraryResponse, LoadModelLibrarySystem, ModelRequestWithHeader, PandaCommandEnum, PandaCommandHeader, RobotHeader}; +use crate::robot::service_types::{ + FR3CommandEnum, LoadModelLibraryArchitecture, LoadModelLibraryRequest, + LoadModelLibraryResponse, LoadModelLibrarySystem, PandaCommandEnum, PandaCommandHeader, + RobotHeader, +}; use crate::FrankaResult; use std::fmt; use std::fmt::Display; @@ -12,24 +16,23 @@ use std::io::Write; use std::mem::size_of; use std::path::Path; - -pub trait LibraryDownloader{ - type Data:RobotData; +pub trait LibraryDownloader { + type Data: RobotData; fn download(network: &mut Network, download_path: &Path) -> FrankaResult<()>; } -pub struct LibraryDownloaderGeneric{ - data: std::marker::PhantomData +pub struct LibraryDownloaderGeneric { + data: std::marker::PhantomData, } -impl LibraryDownloader for LibraryDownloaderGeneric { +impl LibraryDownloader for LibraryDownloaderGeneric { type Data = Data; fn download(network: &mut Network, download_path: &Path) -> FrankaResult<()> { if cfg!(all(target_os = "linux", target_arch = "x86_64")) { - let request= LoadModelLibraryRequest { + let request = LoadModelLibraryRequest { architecture: LoadModelLibraryArchitecture::X64, system: LoadModelLibrarySystem::Linux, }; - let command = Data::create_model_library_request(&mut network.command_id,request); + 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 _response: LoadModelLibraryResponse = @@ -44,9 +47,9 @@ impl LibraryDownloader for LibraryDownloaderGeneric { } else { Err(ModelException { message: - "Your platform is not yet supported for Downloading models. Please use Linux on\ + "Your platform is not yet supported for Downloading models. Please use Linux on\ x86_64 for now" - .to_string(), + .to_string(), }) } } diff --git a/src/network.rs b/src/network.rs index 5af25bc..71df68e 100644 --- a/src/network.rs +++ b/src/network.rs @@ -26,16 +26,31 @@ use nix::sys::socket::sockopt::{KeepAlive, TcpKeepCount, TcpKeepIdle, TcpKeepInt use serde::de::DeserializeOwned; use serde::{Deserialize, Serialize}; -use crate::exception::{FrankaException, FrankaResult}; +use crate::exception::{create_command_exception, FrankaException, FrankaResult}; use crate::gripper::types::{CommandHeader, GripperCommandEnum, GripperCommandHeader}; +use crate::robot::errors::FrankaErrors; +use crate::robot::logger::Record; use crate::robot::robot_state::{FR3State, RobotState}; use crate::robot::service_types::{ - FR3CommandEnum, FR3CommandHeader, LoadModelLibraryRequest, LoadModelLibraryRequestWithHeader, - LoadModelLibraryResponse, LoadModelLibraryStatus, ModelRequestWithHeader, PandaCommandEnum, - PandaCommandHeader, RobotHeader, + AutomaticErrorRecoveryStatusFR3, AutomaticErrorRecoveryStatusPanda, ConnectRequest, + ConnectRequestWithFR3Header, ConnectRequestWithPandaHeader, FR3CommandEnum, FR3CommandHeader, + GetterSetterStatusFR3, GetterSetterStatusPanda, LoadModelLibraryRequest, + LoadModelLibraryRequestWithFR3Header, LoadModelLibraryRequestWithPandaHeader, + LoadModelLibraryResponse, LoadModelLibraryStatus, MoveRequest, MoveRequestWithFR3Header, + MoveRequestWithPandaHeader, MoveStatusFR3, MoveStatusPanda, PandaCommandEnum, + PandaCommandHeader, RobotHeader, SetCartesianImpedanceRequest, + SetCartesianImpedanceRequestWithFR3Header, SetCartesianImpedanceRequestWithPandaHeader, + SetCollisionBehaviorRequest, SetCollisionBehaviorRequestWithFR3Header, + SetCollisionBehaviorRequestWithPandaHeader, SetEeToKRequest, SetEeToKRequestWithFR3Header, + SetEeToKRequestWithPandaHeader, SetGuidingModeRequest, SetGuidingModeRequestWithFR3Header, + SetGuidingModeRequestWithPandaHeader, SetJointImpedanceRequest, + SetJointImpedanceRequestWithFR3Header, SetJointImpedanceRequestWithPandaHeader, SetLoadRequest, + SetLoadRequestWithFR3Header, SetLoadRequestWithPandaHeader, SetNeToEeRequest, + SetNeToEeRequestWithFR3Header, SetNeToEeRequestWithPandaHeader, StopMoveStatusFR3, + StopMoveStatusPanda, FR3_VERSION, PANDA_VERSION, }; -use crate::robot::types::{PandaStateIntern, RobotStateIntern}; -use crate::{gripper, FR3Model, GripperState, PandaModel, PandaState, RobotModel}; +use crate::robot::types::{FR3StateIntern, PandaStateIntern, RobotStateIntern}; +use crate::{FR3Model, PandaModel, PandaState, RobotModel}; const CLIENT: Token = Token(1); @@ -61,10 +76,129 @@ pub trait RobotData: DeviceData { type State: RobotState + From; type StateIntern: Debug + DeserializeOwned + Serialize + RobotStateIntern + 'static; type Model: RobotModel; + 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, - ) -> ModelRequestWithHeader<::Header>; + ) -> 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>; } pub struct PandaData {} @@ -93,19 +227,214 @@ impl RobotData for PandaData { type StateIntern = PandaStateIntern; type Model = PandaModel; - fn create_model_library_request( - mut command_id: &mut u32, - request: LoadModelLibraryRequest, - ) -> ModelRequestWithHeader { - let header = Self::create_header( - &mut command_id, - PandaCommandEnum::LoadModelLibrary, - size_of::(), - ); - ModelRequestWithHeader { header, request } - } + // fn create_model_library_request( + // mut command_id: &mut u32, + // request: LoadModelLibraryRequest, + // ) -> Self::LoadModelRequestWithHeader { + // let header = Self::create_header( + // &mut command_id, + // PandaCommandEnum::LoadModelLibrary, + // size_of::(), + // ); + // Self::LoadModelRequestWithHeader { header, request } + // } type Header = PandaCommandHeader; + type LoadModelRequestWithHeader = LoadModelLibraryRequestWithPandaHeader; + type SetCollisionBehaviorRequestWithHeader = SetCollisionBehaviorRequestWithPandaHeader; + type SetLoadRequestWithHeader = SetLoadRequestWithPandaHeader; + type SetJointImpedanceRequestWithHeader = SetJointImpedanceRequestWithPandaHeader; + type SetCartesianImpedanceRequestWithHeader = SetCartesianImpedanceRequestWithPandaHeader; + type SetGuidingModeRequestWithHeader = SetGuidingModeRequestWithPandaHeader; + type SetEeToKRequestWithHeader = SetEeToKRequestWithPandaHeader; + type SetNeToEeRequestWithHeader = SetNeToEeRequestWithPandaHeader; + type MoveRequestWithHeader = MoveRequestWithPandaHeader; + + 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::(), + ) + } + + type MoveStatus = MoveStatusPanda; + + 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(()) + } + + type GetterSetterStatus = GetterSetterStatusPanda; + + 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_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!", + )), + } + } + + type StopMoveStatus = StopMoveStatusPanda; + type ConnectRequestWithHeader = ConnectRequestWithPandaHeader; + + 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, + } + } + + type AutomaticErrorRecoveryStatus = AutomaticErrorRecoveryStatusPanda; + + 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!")) + } + } + } } impl DeviceData for FR3Data { type CommandHeader = FR3CommandHeader; @@ -124,26 +453,239 @@ impl RobotData for FR3Data { type DeviceData = Self; type Header = FR3CommandHeader; type State = FR3State; - type StateIntern = PandaStateIntern; // todo create fr3stateintern type + type StateIntern = FR3StateIntern; type Model = FR3Model; + type LoadModelRequestWithHeader = LoadModelLibraryRequestWithFR3Header; + type SetCollisionBehaviorRequestWithHeader = SetCollisionBehaviorRequestWithFR3Header; + type SetLoadRequestWithHeader = SetLoadRequestWithFR3Header; + type SetJointImpedanceRequestWithHeader = SetJointImpedanceRequestWithFR3Header; + type SetCartesianImpedanceRequestWithHeader = SetCartesianImpedanceRequestWithFR3Header; + type SetGuidingModeRequestWithHeader = SetGuidingModeRequestWithFR3Header; + type SetEeToKRequestWithHeader = SetEeToKRequestWithFR3Header; + type SetNeToEeRequestWithHeader = SetNeToEeRequestWithFR3Header; + type MoveRequestWithHeader = MoveRequestWithFR3Header; + type MoveStatus = MoveStatusFR3; + type GetterSetterStatus = GetterSetterStatusFR3; - fn create_model_library_request( - mut command_id: &mut u32, - request: LoadModelLibraryRequest, - ) -> ModelRequestWithHeader<<::DeviceData as DeviceData>::CommandHeader> - where - <::DeviceData as DeviceData>::CommandHeader: RobotHeader, - { - let header = Self::create_header( - &mut command_id, - FR3CommandEnum::LoadModelLibrary, - size_of::(), - ); - ModelRequestWithHeader { - header: header, - request: 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 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.", + )) + } + } + + 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.", + )), + } } + + type StopMoveStatus = StopMoveStatusFR3; + type ConnectRequestWithHeader = ConnectRequestWithFR3Header; + + 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, + } + } + + type AutomaticErrorRecoveryStatus = AutomaticErrorRecoveryStatusFR3; + + 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 create_model_library_request( + // mut command_id: &mut u32, + // request: LoadModelLibraryRequest, + // ) -> Self::LoadModelRequestWithHeader + // where + // <::DeviceData as DeviceData>::CommandHeader: RobotHeader, + // { + // // let header = Self::create_header( + // // &mut command_id, + // // FR3CommandEnum::LoadModelLibrary, + // // size_of::(), + // // ); + // // Self::LoadModelRequestWithHeader { + // // header, + // // request, + // // } + // (command_id,request).into() + // } } impl DeviceData for GripperData { @@ -287,13 +829,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 @@ -343,14 +898,14 @@ impl Network { /// /// # Error /// * [`FrankaException`](`crate::exception::FrankaException`) - if the handler returns an exception - pub fn tcp_receive_response( + pub fn tcp_receive_response( &mut self, command_id: u32, handler: F, ) -> 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); @@ -517,7 +1072,7 @@ fn serialize(s: &T) -> Vec { bincode::serialize(s).unwrap() } -fn deserialize(encoded: &[u8]) -> T { +fn deserialize(encoded: &[u8]) -> T { bincode::deserialize(encoded).unwrap() } diff --git a/src/robot.rs b/src/robot.rs index b2959c8..2b9111a 100644 --- a/src/robot.rs +++ b/src/robot.rs @@ -21,17 +21,11 @@ use crate::robot::robot_control::RobotControl; use crate::robot::robot_impl::{RobotImplGeneric, RobotImplementation}; use crate::robot::robot_state::{FR3State, RobotState}; use crate::robot::service_types::{ - AutomaticErrorRecoveryRequestWithHeader, AutomaticErrorRecoveryResponse, - AutomaticErrorRecoveryStatus, GetCartesianLimitRequest, GetCartesianLimitRequestWithHeader, - GetCartesianLimitResponse, GetterSetterStatus, PandaCommandEnum, 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, + GetCartesianLimitRequest, GetCartesianLimitRequestWithPandaHeader, GetCartesianLimitResponse, + GetterSetterStatusPanda, PandaCommandEnum, SetCartesianImpedanceRequest, + SetCollisionBehaviorRequest, SetEeToKRequest, SetFiltersRequest, + SetFiltersRequestWithPandaHeader, SetFiltersResponse, SetGuidingModeRequest, + SetJointImpedanceRequest, SetLoadRequest, SetNeToEeRequest, StopMoveStatusPanda, }; use crate::robot::virtual_wall_cuboid::VirtualWallCuboid; use crate::utils::MotionGenerator; @@ -63,24 +57,85 @@ where { type Data: RobotData; type Rob: RobotImplementation; - fn get_rob(&mut self) -> &mut Self::Rob; + fn get_rob_mut(&mut self) -> &mut Self::Rob; + fn get_rob(&self) -> &Self::Rob; fn get_net(&mut self) -> &mut Network; + /// 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::{Panda, PandaState, FrankaResult}; + /// fn main() -> FrankaResult<()> { + /// let mut robot = Panda::new("robotik-bs.de",None,None)?; + /// let mut count = 0; + /// robot.read(| robot_state:&PandaState | -> bool { + /// println!("{:?}", robot_state); + /// count += 1; + /// count <= 100 + /// }) + /// } + /// ``` + /// # 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 + /// * [`NetworkException`](`crate::exception::FrankaException::NetworkException`) if the connection is lost, e.g. after a timeout. fn read::Data as RobotData>::State) -> bool>( &mut self, mut read_callback: F, ) -> FrankaResult<()> { loop { - let state = self.get_rob().update2(None, None)?; + let state = self.get_rob_mut().update2(None, None)?; if !read_callback(&state) { break; } } Ok(()) } + /// Waits for a robot state update and returns it. + /// + /// # 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<<::Data as RobotData>::State> { - self.get_rob().read_once2() + self.get_rob_mut().read_once2() } - + /// Changes the collision behavior. + /// + /// Set separate torque and force boundaries for acceleration/deceleration and constant velocity + /// movement phases. + /// + /// Forces or torques between lower and upper threshold are shown as contacts in the RobotState. + /// Forces or torques above the upper threshold are registered as collision and cause the robot to + /// stop moving. + /// + /// # Arguments + /// * `lower_torque_thresholds_acceleration` - Contact torque thresholds during + /// acceleration/deceleration for each joint in \[Nm\] + /// * `upper_torque_thresholds_acceleration` - Collision torque thresholds during + /// acceleration/deceleration for each joint in \[Nm\] + /// * `lower_torque_thresholds_nominal` - Contact torque thresholds for each joint in \[Nm\] + /// * `upper_torque_thresholds_nominal` - Collision torque thresholds for each joint in \[Nm\] + /// * `lower_force_thresholds_acceleration` -Contact force thresholds during + /// acceleration/deceleration for (x,y,z,R,P,Y) in \[N\]. + /// * `upper_force_thresholds_acceleration` - Collision force thresholds during + /// acceleration/deceleration for (x,y,z,R,P,Y) in \[N\]. + /// * `lower_force_thresholds_nominal` - Contact force thresholds for (x,y,z,R,P,Y) in \[N\] + /// * `upper_force_thresholds_nominal` - Collision force thresholds for (x,y,z,R,P,Y) in \[N\] + /// # 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. + /// # See also + /// * [`RobotState::cartesian_contact`](`crate::RobotState::cartesian_contact`) + /// * [`RobotState::cartesian_collision`](`crate::RobotState::cartesian_collision`) + /// * [`RobotState::joint_contact`](`crate::RobotState::joint_contact`) + /// * [`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)] fn set_collision_behavior( &mut self, @@ -93,12 +148,9 @@ where lower_force_thresholds_nominal: [f64; 6], upper_force_thresholds_nominal: [f64; 6], ) -> FrankaResult<()> { - let command = SetCollisionBehaviorRequestWithHeader { - header: self.get_net().create_header_for_panda( - PandaCommandEnum::SetCollisionBehavior, - size_of::(), - ), - request: SetCollisionBehaviorRequest::new( + let command = Self::Data::create_set_collision_behavior_request( + &mut self.get_net().command_id, + SetCollisionBehaviorRequest::new( lower_torque_thresholds_acceleration, upper_torque_thresholds_acceleration, lower_torque_thresholds_nominal, @@ -108,41 +160,187 @@ where lower_force_thresholds_nominal, upper_force_thresholds_nominal, ), - }; + ); let command_id: u32 = self.get_net().tcp_send_request(command); - let response: SetCollisionBehaviorResponse = - self.get_net().tcp_blocking_receive_response(command_id); - handle_getter_setter_status(response.status) + let status = self.get_net().tcp_blocking_receive_status(command_id); + Self::Data::handle_getter_setter_status(status) } + /// Sets the impedance for each joint in the internal controller. + /// + /// User-provided torques are not affected by this setting. + /// # Arguments + /// * `K_theta` - Joint impedance values ![K_{\theta}](https://latex.codecogs.com/png.latex?K_{\theta}). + /// # 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_joint_impedance(&mut self, K_theta: [f64; 7]) -> FrankaResult<()> { - let command = SetJointImpedanceRequestWithHeader { - header: self.get_net().create_header_for_panda( - PandaCommandEnum::SetJointImpedance, - size_of::(), - ), - request: SetJointImpedanceRequest::new(K_theta), - }; + let command = Self::Data::create_set_joint_impedance_request( + &mut self.get_net().command_id, + SetJointImpedanceRequest::new(K_theta), + ); let command_id: u32 = self.get_net().tcp_send_request(command); - let response: SetJointImpedanceResponse = - self.get_net().tcp_blocking_receive_response(command_id); - handle_getter_setter_status(response.status) + let status = self.get_net().tcp_blocking_receive_status(command_id); + Self::Data::handle_getter_setter_status(status) } - + /// Sets the Cartesian impedance for (x, y, z, roll, pitch, yaw) in the internal controller. + /// + /// User-provided torques are not affected by this setting. + /// # Arguments + /// * `K_x` - Cartesian impedance values + /// + /// ![K_x=(x \in \[10,3000\] \frac{N}{m}, y \in \[10,3000\] \frac{N}{m}, z \in \[10,3000\] \frac{N}{m}, R \in \[1,300\] \frac{Nm}{rad}, P \in \[1,300\] \frac{Nm}{rad}, Y \in \[1,300\] \frac{Nm}{rad})](https://latex.codecogs.com/png.latex?K_x=(x&space;\in&space;[10,3000]&space;\frac{N}{m},&space;y&space;\in&space;[10,3000]&space;\frac{N}{m},&space;z&space;\in&space;[10,3000]&space;\frac{N}{m},&space;R&space;\in&space;[1,300]&space;\frac{Nm}{rad},&space;P&space;\in&space;[1,300]&space;\frac{Nm}{rad},&space;Y&space;\in&space;[1,300]&space;\frac{Nm}{rad})) + /// # 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_cartesian_impedance(&mut self, K_x: [f64; 6]) -> FrankaResult<()> { - let command = SetCartesianImpedanceRequestWithHeader { - header: self.get_net().create_header_for_panda( - PandaCommandEnum::SetCartesianImpedance, - size_of::(), - ), - request: SetCartesianImpedanceRequest::new(K_x), - }; + let command = Self::Data::create_set_cartesian_impedance_request( + &mut self.get_net().command_id, + SetCartesianImpedanceRequest::new(K_x), + ); + let command_id: u32 = self.get_net().tcp_send_request(command); + let status = self.get_net().tcp_blocking_receive_status(command_id); + Self::Data::handle_getter_setter_status(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)] + fn set_load( + &mut self, + load_mass: f64, + F_x_Cload: [f64; 3], + load_inertia: [f64; 9], + ) -> FrankaResult<()> { + let command = Self::Data::create_set_load_request( + &mut self.get_net().command_id, + SetLoadRequest::new(load_mass, F_x_Cload, load_inertia), + ); let command_id: u32 = self.get_net().tcp_send_request(command); - let response: SetCartesianImpedanceResponse = - self.get_net().tcp_blocking_receive_response(command_id); - handle_getter_setter_status(response.status) + let status = self.get_net().tcp_blocking_receive_status(command_id); + Self::Data::handle_getter_setter_status(status) + } + + /// Locks or unlocks guiding mode movement in (x, y, z, roll, pitch, yaw). + /// + /// If a flag is set to true, movement is unlocked. + /// # Note + /// Guiding mode can be enabled by pressing the two opposing buttons near the robot's flange. + /// # Arguments + /// * `guiding_mode` - Unlocked movement in (x, y, z, R, P, Y) in guiding mode. + /// * `elbow` - True if the elbow is free in guiding mode, false otherwise. + /// # 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_guiding_mode(&mut self, guiding_mode: [bool; 6], elbow: bool) -> FrankaResult<()> { + let command = Self::Data::create_set_guiding_mode_request( + &mut self.get_net().command_id, + SetGuidingModeRequest::new(guiding_mode, elbow), + ); + let command_id: u32 = self.get_net().tcp_send_request(command); + let status = self.get_net().tcp_blocking_receive_status(command_id); + Self::Data::handle_getter_setter_status(status) + } + + /// Sets the transformation ![^{EE}T_K](https://latex.codecogs.com/png.latex?^{EE}T_K) from end effector frame to stiffness frame. + /// + /// The transformation matrix is represented as a vectorized 4x4 matrix in column-major format. + /// # Arguments + /// * `EE_T_K` - Vectorized EE-to-K transformation matrix ![^{EE}T_K](https://latex.codecogs.com/png.latex?^{EE}T_K), 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. + /// + /// See [Stiffness frame K](#stiffness-frame-k) for an explanation of the stiffness frame. + #[allow(non_snake_case)] + fn set_K(&mut self, EE_T_K: [f64; 16]) -> FrankaResult<()> { + let command = Self::Data::create_set_ee_to_k_request( + &mut self.get_net().command_id, + SetEeToKRequest::new(EE_T_K), + ); + let command_id: u32 = self.get_net().tcp_send_request(command); + let status = self.get_net().tcp_blocking_receive_status(command_id); + Self::Data::handle_getter_setter_status(status) + } + + /// Sets the transformation ![^{NE}T_{EE}](https://latex.codecogs.com/png.latex?^{NE}T_{EE}) from nominal end effector to end effector frame. + /// + /// The transformation matrix is represented as a vectorized 4x4 matrix in column-major format. + /// # Arguments + /// * `NE_T_EE` - Vectorized NE-to-EE transformation matrix ![^{NE}T_{EE}](https://latex.codecogs.com/png.latex?^{NE}T_{EE}), 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. + /// + /// # See also + /// * [`RobotState::NE_T_EE`](`crate::RobotState::NE_T_EE`) + /// * [`RobotState::O_T_EE`](`crate::RobotState::O_T_EE`) + /// * [`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)] + fn set_EE(&mut self, NE_T_EE: [f64; 16]) -> FrankaResult<()> { + let command = Self::Data::create_set_ne_to_ee_request( + &mut self.get_net().command_id, + SetNeToEeRequest::new(NE_T_EE), + ); + let command_id: u32 = self.get_net().tcp_send_request(command); + let status = self.get_net().tcp_blocking_receive_status(command_id); + Self::Data::handle_getter_setter_status(status) + } + + ///Runs automatic error recovery on the robot. + /// + /// Automatic error recovery e.g. resets the robot after a collision occurred. + /// # 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 automatic_error_recovery(&mut self) -> FrankaResult<()> { + let command = + Self::Data::create_automatic_error_recovery_request(&mut self.get_net().command_id); + let command_id: u32 = self.get_net().tcp_send_request(command); + let status = self.get_net().tcp_blocking_receive_status(command_id); + Self::Data::handle_automatic_error_recovery_status(status) + } + + /// Stops all currently running motions. + /// + /// If a control or motion generator loop is running in another thread, it will be preempted + /// with a [`ControlException`](`crate::exception::FrankaException::ControlException`). + /// # 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 stop(&mut self) -> FrankaResult<()> { + let command = Self::Data::create_stop_request(&mut self.get_net().command_id); + let command_id: u32 = self.get_net().tcp_send_request(command); + let status: StopMoveStatusPanda = self.get_net().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 control_motion_intern< @@ -162,7 +360,7 @@ where 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( - self.get_rob(), + self.get_rob_mut(), controller_mode, motion_generator_callback, limit_rate, @@ -170,9 +368,161 @@ where )?; control_loop.run() } + /// 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. + fn control_joint_positions< + F: FnMut(&<::Data as RobotData>::State, &Duration) -> JointPositions, + CM: Into>, + L: Into>, + CF: Into>, + >( + &mut self, + motion_generator_callback: F, + controller_mode: CM, + limit_rate: L, + cutoff_frequency: CF, + ) -> FrankaResult<()> { + self.control_motion_intern( + motion_generator_callback, + 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. + fn control_joint_velocities< + F: FnMut(&<::Data as RobotData>::State, &Duration) -> JointVelocities, + CM: Into>, + L: Into>, + CF: Into>, + >( + &mut self, + motion_generator_callback: F, + controller_mode: CM, + limit_rate: L, + cutoff_frequency: CF, + ) -> FrankaResult<()> { + self.control_motion_intern( + motion_generator_callback, + 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. + fn control_cartesian_pose< + F: FnMut(&<::Data as RobotData>::State, &Duration) -> CartesianPose, + CM: Into>, + L: Into>, + CF: Into>, + >( + &mut self, + motion_generator_callback: F, + controller_mode: CM, + limit_rate: L, + cutoff_frequency: CF, + ) -> FrankaResult<()> { + self.control_motion_intern( + motion_generator_callback, + controller_mode.into(), + limit_rate.into(), + cutoff_frequency.into(), + ) + } - fn control_cartesian_pose< - F: FnMut(&<::Data as RobotData>::State, &Duration) -> CartesianPose, + /// 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. + fn control_cartesian_velocities< + F: FnMut(&<::Data as RobotData>::State, &Duration) -> CartesianVelocities, CM: Into>, L: Into>, CF: Into>, @@ -210,7 +560,7 @@ where let limit_rate = limit_rate.unwrap_or(true); let cutoff_frequency = cutoff_frequency.unwrap_or(DEFAULT_CUTOFF_FREQUENCY); let mut control_loop = ControlLoop::new( - self.get_rob(), + self.get_rob_mut(), control_callback, motion_generator_callback, limit_rate, @@ -451,7 +801,42 @@ where /// * [`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. fn load_model(&mut self, persistent: bool) -> FrankaResult<::Model> { - self.get_rob().load_model2(persistent) + self.get_rob_mut().load_model2(persistent) + } + + /// 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<()> { + self.set_collision_behavior( + [20.; 7], [20.; 7], [10.; 7], [10.; 7], [20.; 6], [20.; 6], [10.; 6], [10.; 6], + )?; + self.set_joint_impedance([3000., 3000., 3000., 2500., 2500., 2000., 2000.])?; + 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. + 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), + ) + } + /// Returns the software version reported by the connected server. + /// + /// # Return + /// Software version of the connected server. + fn server_version(&self) -> u16 { + self.get_rob().server_version2() } } @@ -518,9 +903,13 @@ impl Robot for Panda { fn get_net(&mut self) -> &mut Network { &mut self.robimpl.network } - fn get_rob(&mut self) -> &mut Self::Rob { + fn get_rob_mut(&mut self) -> &mut Self::Rob { &mut self.robimpl } + + fn get_rob(&self) -> &Self::Rob { + &self.robimpl + } } pub struct FR3 { @@ -533,9 +922,13 @@ impl Robot for FR3 { fn get_net(&mut self) -> &mut Network { &mut self.robimpl.network } - fn get_rob(&mut self) -> &mut Self::Rob { + fn get_rob_mut(&mut self) -> &mut Self::Rob { &mut self.robimpl } + + fn get_rob(&self) -> &Self::Rob { + &self.robimpl + } } impl FR3 { @@ -582,309 +975,27 @@ impl Panda { /// ``` /// # 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::Panda, - franka_address, - service_types::COMMAND_PORT, - ) - .map_err(|_| FrankaException::NetworkException { - message: "Connection could not be established".to_string(), - })?; - Ok(Panda { - robimpl: ::Rob::new(network, log_size, realtime_config)?, - }) - } - /// 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::{Panda, PandaState, FrankaResult}; - /// fn main() -> FrankaResult<()> { - /// let mut robot = Panda::new("robotik-bs.de",None,None)?; - /// let mut count = 0; - /// robot.read(| robot_state:&PandaState | -> bool { - /// println!("{:?}", robot_state); - /// count += 1; - /// count <= 100 - /// }) - /// } - /// ``` - /// # 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 - /// * [`NetworkException`](`crate::exception::FrankaException::NetworkException`) if the connection is lost, e.g. after a timeout. - // pub fn read bool>( - // &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. - /// - /// # 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. - // pub fn read_once(&mut self) -> FrankaResult { - // self.robimpl.read_once() - // } - - /// Changes the collision behavior. - /// - /// Set separate torque and force boundaries for acceleration/deceleration and constant velocity - /// movement phases. - /// - /// Forces or torques between lower and upper threshold are shown as contacts in the RobotState. - /// Forces or torques above the upper threshold are registered as collision and cause the robot to - /// stop moving. - /// - /// # Arguments - /// * `lower_torque_thresholds_acceleration` - Contact torque thresholds during - /// acceleration/deceleration for each joint in \[Nm\] - /// * `upper_torque_thresholds_acceleration` - Collision torque thresholds during - /// acceleration/deceleration for each joint in \[Nm\] - /// * `lower_torque_thresholds_nominal` - Contact torque thresholds for each joint in \[Nm\] - /// * `upper_torque_thresholds_nominal` - Collision torque thresholds for each joint in \[Nm\] - /// * `lower_force_thresholds_acceleration` -Contact force thresholds during - /// acceleration/deceleration for (x,y,z,R,P,Y) in \[N\]. - /// * `upper_force_thresholds_acceleration` - Collision force thresholds during - /// acceleration/deceleration for (x,y,z,R,P,Y) in \[N\]. - /// * `lower_force_thresholds_nominal` - Contact force thresholds for (x,y,z,R,P,Y) in \[N\] - /// * `upper_force_thresholds_nominal` - Collision force thresholds for (x,y,z,R,P,Y) in \[N\] - /// # 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. - /// # See also - /// * [`RobotState::cartesian_contact`](`crate::RobotState::cartesian_contact`) - /// * [`RobotState::cartesian_collision`](`crate::RobotState::cartesian_collision`) - /// * [`RobotState::joint_contact`](`crate::RobotState::joint_contact`) - /// * [`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( - // &mut self, - // 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 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) - // } - - /// Sets the impedance for each joint in the internal controller. - /// - /// User-provided torques are not affected by this setting. - /// # Arguments - /// * `K_theta` - Joint impedance values ![K_{\theta}](https://latex.codecogs.com/png.latex?K_{\theta}). - /// # 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_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) - // } - - /// Sets the Cartesian impedance for (x, y, z, roll, pitch, yaw) in the internal controller. - /// - /// User-provided torques are not affected by this setting. - /// # Arguments - /// * `K_x` - Cartesian impedance values - /// - /// ![K_x=(x \in \[10,3000\] \frac{N}{m}, y \in \[10,3000\] \frac{N}{m}, z \in \[10,3000\] \frac{N}{m}, R \in \[1,300\] \frac{Nm}{rad}, P \in \[1,300\] \frac{Nm}{rad}, Y \in \[1,300\] \frac{Nm}{rad})](https://latex.codecogs.com/png.latex?K_x=(x&space;\in&space;[10,3000]&space;\frac{N}{m},&space;y&space;\in&space;[10,3000]&space;\frac{N}{m},&space;z&space;\in&space;[10,3000]&space;\frac{N}{m},&space;R&space;\in&space;[1,300]&space;\frac{Nm}{rad},&space;P&space;\in&space;[1,300]&space;\frac{Nm}{rad},&space;Y&space;\in&space;[1,300]&space;\frac{Nm}{rad})) - /// # 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_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) - // } - /// Locks or unlocks guiding mode movement in (x, y, z, roll, pitch, yaw). - /// - /// If a flag is set to true, movement is unlocked. - /// # Note - /// Guiding mode can be enabled by pressing the two opposing buttons near the robot's flange. - /// # Arguments - /// * `guiding_mode` - Unlocked movement in (x, y, z, R, P, Y) in guiding mode. - /// * `elbow` - True if the elbow is free in guiding mode, false otherwise. - /// # 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_panda( - PandaCommandEnum::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) - } - - /// Sets the transformation ![^{EE}T_K](https://latex.codecogs.com/png.latex?^{EE}T_K) from end effector frame to stiffness frame. - /// - /// The transformation matrix is represented as a vectorized 4x4 matrix in column-major format. - /// # Arguments - /// * `EE_T_K` - Vectorized EE-to-K transformation matrix ![^{EE}T_K](https://latex.codecogs.com/png.latex?^{EE}T_K), 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. - /// - /// 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_panda( - PandaCommandEnum::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) - } - - /// Sets the transformation ![^{NE}T_{EE}](https://latex.codecogs.com/png.latex?^{NE}T_{EE}) from nominal end effector to end effector frame. - /// - /// The transformation matrix is represented as a vectorized 4x4 matrix in column-major format. - /// # Arguments - /// * `NE_T_EE` - Vectorized NE-to-EE transformation matrix ![^{NE}T_{EE}](https://latex.codecogs.com/png.latex?^{NE}T_{EE}), 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. - /// - /// # See also - /// * [`RobotState::NE_T_EE`](`crate::RobotState::NE_T_EE`) - /// * [`RobotState::O_T_EE`](`crate::RobotState::O_T_EE`) - /// * [`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_panda( - PandaCommandEnum::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_panda( - PandaCommandEnum::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) + /// * 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::Panda, + franka_address, + service_types::COMMAND_PORT, + ) + .map_err(|_| FrankaException::NetworkException { + message: "Connection could not be established".to_string(), + })?; + Ok(Panda { + robimpl: ::Rob::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. @@ -911,10 +1022,10 @@ impl Panda { cartesian_velocity_filter_frequency: f64, controller_filter_frequency: f64, ) -> FrankaResult<()> { - let command = SetFiltersRequestWithHeader { + let command = SetFiltersRequestWithPandaHeader { header: self.robimpl.network.create_header_for_panda( PandaCommandEnum::SetFilters, - size_of::(), + size_of::(), ), request: SetFiltersRequest::new( joint_position_filter_frequency, @@ -929,83 +1040,9 @@ impl Panda { .robimpl .network .tcp_blocking_receive_response(command_id); - handle_getter_setter_status(response.status) - } - - ///Runs automatic error recovery on the robot. - /// - /// Automatic error recovery e.g. resets the robot after a collision occurred. - /// # 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_panda( - PandaCommandEnum::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!")) - } - } + PandaData::handle_getter_setter_status(response.status) } - /// Stops all currently running motions. - /// - /// If a control or motion generator loop is running in another thread, it will be preempted - /// with a [`ControlException`](`crate::exception::FrankaException::ControlException`). - /// # 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_panda( - PandaCommandEnum::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!", - )), - } - } /// Returns the parameters of a virtual wall. /// # Arguments /// * `id` - ID of the virtual wall. @@ -1015,10 +1052,10 @@ impl Panda { /// * [`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 { + let command = GetCartesianLimitRequestWithPandaHeader { header: self.robimpl.network.create_header_for_panda( PandaCommandEnum::GetCartesianLimit, - size_of::(), + size_of::(), ), request: GetCartesianLimitRequest::new(id), }; @@ -1028,234 +1065,17 @@ impl Panda { .network .tcp_blocking_receive_response(command_id); match &response.status { - GetterSetterStatus::Success => Ok(VirtualWallCuboid::new(id, response)), - GetterSetterStatus::CommandNotPossibleRejected => Err(create_command_exception( + GetterSetterStatusPanda::Success => Ok(VirtualWallCuboid::new(id, response)), + GetterSetterStatusPanda::CommandNotPossibleRejected => Err(create_command_exception( "libfranka-rs: command rejected: command not possible in current mode", )), - GetterSetterStatus::InvalidArgumentRejected => Err(create_command_exception( + GetterSetterStatusPanda::InvalidArgumentRejected => Err(create_command_exception( "libfranka-rs: command rejected: invalid argument!", )), } } - /// 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< - F: FnMut(&PandaState, &Duration) -> JointPositions, - CM: Into>, - L: Into>, - CF: Into>, - >( - &mut self, - motion_generator_callback: F, - controller_mode: CM, - limit_rate: L, - cutoff_frequency: CF, - ) -> FrankaResult<()> { - self.control_motion_intern( - motion_generator_callback, - 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< - F: FnMut(&PandaState, &Duration) -> JointVelocities, - CM: Into>, - L: Into>, - CF: Into>, - >( - &mut self, - motion_generator_callback: F, - controller_mode: CM, - limit_rate: L, - cutoff_frequency: CF, - ) -> FrankaResult<()> { - self.control_motion_intern( - motion_generator_callback, - 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< - // F: FnMut(&PandaState, &Duration) -> CartesianPose, - // CM: Into>, - // L: Into>, - // CF: Into>, - // >( - // &mut self, - // motion_generator_callback: F, - // controller_mode: CM, - // limit_rate: L, - // cutoff_frequency: CF, - // ) -> FrankaResult<()> { - // self.control_motion_intern( - // motion_generator_callback, - // 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< - F: FnMut(&PandaState, &Duration) -> CartesianVelocities, - CM: Into>, - L: Into>, - CF: Into>, - >( - &mut self, - motion_generator_callback: F, - controller_mode: CM, - limit_rate: L, - cutoff_frequency: CF, - ) -> FrankaResult<()> { - self.control_motion_intern( - motion_generator_callback, - controller_mode.into(), - limit_rate.into(), - cutoff_frequency.into(), - ) - } - - /// 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<()> { - self.set_collision_behavior( - [20.; 7], [20.; 7], [10.; 7], [10.; 7], [20.; 6], [20.; 6], [10.; 6], [10.; 6], - )?; - self.set_joint_impedance([3000., 3000., 3000., 2500., 2500., 2000., 2000.])?; - 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), - ) - } - /// 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() - } } -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!", - )), - } -} #[cfg(test)] mod tests { use mockall::{automock, predicate::*}; @@ -1268,14 +1088,15 @@ 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, PandaCommandEnum, PandaCommandHeader, - SetCollisionBehaviorRequest, SetCollisionBehaviorRequestWithHeader, - SetCollisionBehaviorResponse, COMMAND_PORT, VERSION, + ConnectRequestWithPandaHeader, ConnectResponsePanda, ConnectStatus, FR3CommandEnum, + FR3CommandHeader, GetterSetterStatusPanda, MoveControllerMode, MoveDeviation, + MoveMotionGeneratorMode, MoveRequest, MoveRequestWithPandaHeader, MoveResponse, + MoveStatusPanda, PandaCommandEnum, PandaCommandHeader, SetCollisionBehaviorRequest, + SetCollisionBehaviorRequestWithFR3Header, SetCollisionBehaviorRequestWithPandaHeader, + SetterResponseFR3, COMMAND_PORT, FR3_VERSION, PANDA_VERSION, }; use crate::robot::types::PandaStateIntern; - use crate::robot::Robot; + use crate::robot::{Robot, FR3}; use crate::{Finishable, FrankaResult, JointPositions, Panda, PandaState, RealtimeConfig}; use bincode::{deserialize, serialize, serialized_size}; use std::iter::FromIterator; @@ -1409,18 +1230,18 @@ mod tests { fn receive_robot_connect_request), G: Fn(&mut Vec)>( &self, tcp_socket: &mut Socket, - ) -> ConnectRequestWithHeader { + ) -> ConnectRequestWithPandaHeader { let mut bytes = vec![0 as u8; 100]; (tcp_socket.receive_bytes)(&mut bytes); - let request: ConnectRequestWithHeader = deserialize(bytes.as_slice()).unwrap(); + let request: ConnectRequestWithPandaHeader = deserialize(bytes.as_slice()).unwrap(); return request; } fn send_robot_connect_response), G: Fn(&mut Vec)>( &self, - request: ConnectRequestWithHeader, + request: ConnectRequestWithPandaHeader, tcp_socket: &mut Socket, ) { - let mut response = ConnectResponse { + let mut response = ConnectResponsePanda { header: PandaCommandHeader { command: PandaCommandEnum::Connect, command_id: request.get_command_message_id(), @@ -1462,11 +1283,11 @@ mod tests { lower_force_thresholds_nominal: [f64; 6], upper_force_thresholds_nominal: [f64; 6]| { counter += 1; - SetCollisionBehaviorRequestWithHeader { - header: PandaCommandHeader::new( - PandaCommandEnum::SetCollisionBehavior, + SetCollisionBehaviorRequestWithFR3Header { + header: FR3CommandHeader::new( + FR3CommandEnum::SetCollisionBehavior, counter, - size_of::() as u32, + size_of::() as u32, ), request: SetCollisionBehaviorRequest::new( lower_torque_thresholds_acceleration, @@ -1489,7 +1310,7 @@ mod tests { )); let requests_server = requests.clone(); 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; @@ -1502,15 +1323,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: PandaCommandHeader::new( - PandaCommandEnum::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() @@ -1521,8 +1343,8 @@ mod tests { }); { std::thread::sleep(Duration::from_secs_f64(0.01)); - let mut robot = Panda::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) @@ -1536,11 +1358,11 @@ mod tests { #[test] fn fail_start_motion_test() { - let requests = Arc::new(vec![MoveRequestWithHeader { + 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, @@ -1559,7 +1381,7 @@ mod tests { }]); let requests_server = requests.clone(); 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; @@ -1567,22 +1389,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: PandaCommandHeader::new( - PandaCommandEnum::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); @@ -1592,7 +1411,7 @@ mod tests { { std::thread::sleep(Duration::from_secs_f64(0.01)); let mut robot = - Panda::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( |_, _| { @@ -1620,7 +1439,7 @@ 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()); @@ -1647,7 +1466,7 @@ 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()); @@ -1657,7 +1476,7 @@ mod tests { { std::thread::sleep(Duration::from_secs_f64(0.01)); - let mut robot = Panda::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(); @@ -1667,7 +1486,7 @@ 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()); @@ -1676,7 +1495,7 @@ mod tests { }); { std::thread::sleep(Duration::from_secs_f64(0.01)); - let mut robot = Panda::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_types.rs b/src/robot/control_types.rs index ee93ff0..90d5b13 100644 --- a/src/robot/control_types.rs +++ b/src/robot/control_types.rs @@ -42,7 +42,6 @@ pub enum RealtimeConfig { /// /// Used to determine whether to terminate a loop after the control callback has returned. pub trait ConvertMotion { - /// converts the motion type to a MotionGeneratorCommand and applies rate limiting and filtering fn convert_motion( &self, @@ -128,7 +127,7 @@ impl Torques { } } -impl Finishable for Torques{ +impl Finishable for Torques { fn is_finished(&self) -> bool { self.motion_finished } @@ -141,9 +140,7 @@ impl Finishable for Torques{ } } -impl ConvertMotion for Torques { - - +impl ConvertMotion for Torques { #[allow(unused_variables)] //todo pull convert motion out of the Finishable trait fn convert_motion( @@ -203,7 +200,7 @@ impl JointPositions { } } } -impl Finishable for JointPositions{ +impl Finishable for JointPositions { fn is_finished(&self) -> bool { self.motion_finished } @@ -213,45 +210,12 @@ impl Finishable for JointPositions{ fn motion_finished(mut self) -> Self { self.set_motion_finished(true); self - }} -impl ConvertMotion for JointPositions { - - fn convert_motion( - &self, - robot_state: &PandaState, - command: &mut MotionGeneratorCommand, - cutoff_frequency: f64, - limit_rate: bool, - ) { - command.q_c = self.q; - if cutoff_frequency < MAX_CUTOFF_FREQUENCY { - for i in 0..7 { - command.q_c[i] = low_pass_filter( - DELTA_T, - command.q_c[i], - robot_state.q_d[i], - cutoff_frequency, - ); - } - } - if limit_rate { - command.q_c = limit_rate_joint_positions( - &MAX_JOINT_VELOCITY, - &MAX_JOINT_ACCELERATION, - &MAX_JOINT_JERK, - &command.q_c, - &robot_state.q_d, - &robot_state.dq_d, - &robot_state.ddq_d, - ); - } - command.q_c.iter().for_each(|x| assert!(x.is_finite())); } } -impl ConvertMotion for JointPositions { +impl ConvertMotion for JointPositions { fn convert_motion( &self, - robot_state: &FR3State, + robot_state: &PandaState, command: &mut MotionGeneratorCommand, cutoff_frequency: f64, limit_rate: bool, @@ -281,6 +245,39 @@ impl ConvertMotion for JointPositions { command.q_c.iter().for_each(|x| assert!(x.is_finite())); } } +// impl ConvertMotion for JointPositions { +// fn convert_motion( +// &self, +// robot_state: &FR3State, +// command: &mut MotionGeneratorCommand, +// cutoff_frequency: f64, +// limit_rate: bool, +// ) { +// command.q_c = self.q; +// if cutoff_frequency < MAX_CUTOFF_FREQUENCY { +// for i in 0..7 { +// command.q_c[i] = low_pass_filter( +// DELTA_T, +// command.q_c[i], +// robot_state.q_d[i], +// cutoff_frequency, +// ); +// } +// } +// if limit_rate { +// command.q_c = limit_rate_joint_positions( +// &MAX_JOINT_VELOCITY, +// &MAX_JOINT_ACCELERATION, +// &MAX_JOINT_JERK, +// &command.q_c, +// &robot_state.q_d, +// &robot_state.dq_d, +// &robot_state.ddq_d, +// ); +// } +// command.q_c.iter().for_each(|x| assert!(x.is_finite())); +// } +// } impl MotionGeneratorTrait for JointPositions { fn get_motion_generator_mode() -> MoveMotionGeneratorMode { @@ -314,7 +311,7 @@ impl JointVelocities { } } } -impl Finishable for JointVelocities{ +impl Finishable for JointVelocities { fn is_finished(&self) -> bool { self.motion_finished } @@ -327,7 +324,6 @@ impl Finishable for JointVelocities{ } } impl ConvertMotion for JointVelocities { - fn convert_motion( &self, robot_state: &PandaState, @@ -360,38 +356,38 @@ impl ConvertMotion for JointVelocities { } } -impl ConvertMotion for JointVelocities { - fn convert_motion( - &self, - robot_state: &FR3State, - command: &mut MotionGeneratorCommand, - cutoff_frequency: f64, - limit_rate: bool, - ) { - command.dq_c = self.dq; - if cutoff_frequency < MAX_CUTOFF_FREQUENCY { - for i in 0..7 { - command.dq_c[i] = low_pass_filter( - DELTA_T, - command.dq_c[i], - robot_state.dq_d[i], - cutoff_frequency, - ); - } - } - if limit_rate { - command.dq_c = limit_rate_joint_velocities( - &MAX_JOINT_VELOCITY, - &MAX_JOINT_ACCELERATION, - &MAX_JOINT_JERK, - &command.dq_c, - &robot_state.dq_d, - &robot_state.ddq_d, - ); - } - command.dq_c.iter().for_each(|x| assert!(x.is_finite())); - } -} +// impl ConvertMotion for JointVelocities { +// fn convert_motion( +// &self, +// robot_state: &FR3State, +// command: &mut MotionGeneratorCommand, +// cutoff_frequency: f64, +// limit_rate: bool, +// ) { +// command.dq_c = self.dq; +// if cutoff_frequency < MAX_CUTOFF_FREQUENCY { +// for i in 0..7 { +// command.dq_c[i] = low_pass_filter( +// DELTA_T, +// command.dq_c[i], +// robot_state.dq_d[i], +// cutoff_frequency, +// ); +// } +// } +// if limit_rate { +// command.dq_c = limit_rate_joint_velocities( +// &MAX_JOINT_VELOCITY, +// &MAX_JOINT_ACCELERATION, +// &MAX_JOINT_JERK, +// &command.dq_c, +// &robot_state.dq_d, +// &robot_state.ddq_d, +// ); +// } +// command.dq_c.iter().for_each(|x| assert!(x.is_finite())); +// } +// } impl MotionGeneratorTrait for JointVelocities { fn get_motion_generator_mode() -> MoveMotionGeneratorMode { @@ -466,7 +462,7 @@ impl CartesianPose { } } -impl Finishable for CartesianPose{ +impl Finishable for CartesianPose { fn is_finished(&self) -> bool { self.motion_finished } @@ -480,7 +476,6 @@ impl Finishable for CartesianPose{ } impl ConvertMotion for CartesianPose { - fn convert_motion( &self, robot_state: &PandaState, @@ -544,70 +539,70 @@ impl ConvertMotion for CartesianPose { } } -impl ConvertMotion for CartesianPose { - - fn convert_motion( - &self, - robot_state: &FR3State, - command: &mut MotionGeneratorCommand, - cutoff_frequency: f64, - limit_rate: bool, - ) { - command.O_T_EE_c = self.O_T_EE; - if cutoff_frequency < MAX_CUTOFF_FREQUENCY { - command.O_T_EE_c = cartesian_low_pass_filter( - DELTA_T, - &command.O_T_EE_c, - &robot_state.O_T_EE_c, - cutoff_frequency, - ); - } - - 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, - &command.O_T_EE_c, - &robot_state.O_T_EE_c, - &robot_state.O_dP_EE_c, - &robot_state.O_ddP_EE_c, - ); - } - check_matrix(&command.O_T_EE_c); - - if self.has_elbow() { - command.valid_elbow = true; - command.elbow_c = self.elbow.unwrap(); - if cutoff_frequency < MAX_CUTOFF_FREQUENCY { - command.elbow_c[0] = low_pass_filter( - DELTA_T, - command.elbow_c[0], - robot_state.elbow_c[0], - cutoff_frequency, - ); - } - if limit_rate { - command.elbow_c[0] = limit_rate_position( - MAX_ELBOW_VELOCITY, - MAX_ELBOW_ACCELERATION, - MAX_ELBOW_JERK, - command.elbow_c[0], - robot_state.elbow_c[0], - robot_state.delbow_c[0], - robot_state.ddelbow_c[0], - ); - } - CartesianPose::check_elbow(&command.elbow_c); - } else { - command.valid_elbow = false; - command.elbow_c = [0.; 2]; - } - } -} +// impl ConvertMotion for CartesianPose { +// +// fn convert_motion( +// &self, +// robot_state: &FR3State, +// command: &mut MotionGeneratorCommand, +// cutoff_frequency: f64, +// limit_rate: bool, +// ) { +// command.O_T_EE_c = self.O_T_EE; +// if cutoff_frequency < MAX_CUTOFF_FREQUENCY { +// command.O_T_EE_c = cartesian_low_pass_filter( +// DELTA_T, +// &command.O_T_EE_c, +// &robot_state.O_T_EE_c, +// cutoff_frequency, +// ); +// } +// +// 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, +// &command.O_T_EE_c, +// &robot_state.O_T_EE_c, +// &robot_state.O_dP_EE_c, +// &robot_state.O_ddP_EE_c, +// ); +// } +// check_matrix(&command.O_T_EE_c); +// +// if self.has_elbow() { +// command.valid_elbow = true; +// command.elbow_c = self.elbow.unwrap(); +// if cutoff_frequency < MAX_CUTOFF_FREQUENCY { +// command.elbow_c[0] = low_pass_filter( +// DELTA_T, +// command.elbow_c[0], +// robot_state.elbow_c[0], +// cutoff_frequency, +// ); +// } +// if limit_rate { +// command.elbow_c[0] = limit_rate_position( +// MAX_ELBOW_VELOCITY, +// MAX_ELBOW_ACCELERATION, +// MAX_ELBOW_JERK, +// command.elbow_c[0], +// robot_state.elbow_c[0], +// robot_state.delbow_c[0], +// robot_state.ddelbow_c[0], +// ); +// } +// CartesianPose::check_elbow(&command.elbow_c); +// } else { +// command.valid_elbow = false; +// command.elbow_c = [0.; 2]; +// } +// } +// } impl MotionGeneratorTrait for CartesianPose { fn get_motion_generator_mode() -> MoveMotionGeneratorMode { @@ -658,7 +653,7 @@ impl CartesianVelocities { self.elbow.is_some() } } -impl Finishable for CartesianVelocities{ +impl Finishable for CartesianVelocities { fn is_finished(&self) -> bool { self.motion_finished } @@ -671,7 +666,6 @@ impl Finishable for CartesianVelocities{ } } impl ConvertMotion for CartesianVelocities { - fn convert_motion( &self, robot_state: &PandaState, @@ -738,73 +732,73 @@ impl ConvertMotion for CartesianVelocities { } } -impl ConvertMotion for CartesianVelocities { - - fn convert_motion( - &self, - robot_state: &FR3State, - command: &mut MotionGeneratorCommand, - cutoff_frequency: f64, - limit_rate: bool, - ) { - command.O_dP_EE_c = self.O_dP_EE; - if cutoff_frequency < MAX_CUTOFF_FREQUENCY { - for i in 0..6 { - command.O_dP_EE_c[i] = low_pass_filter( - DELTA_T, - command.O_dP_EE_c[i], - robot_state.O_dP_EE_c[i], - cutoff_frequency, - ); - } - } - 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, - &command.O_dP_EE_c, - &robot_state.O_dP_EE_c, - &robot_state.O_ddP_EE_c, - ); - } - command - .O_dP_EE_c - .iter() - .for_each(|x| assert!(x.is_finite())); - - if self.has_elbow() { - command.valid_elbow = true; - command.elbow_c = self.elbow.unwrap(); - if cutoff_frequency < MAX_CUTOFF_FREQUENCY { - command.elbow_c[0] = low_pass_filter( - DELTA_T, - command.elbow_c[0], - robot_state.elbow_c[0], - cutoff_frequency, - ); - } - if limit_rate { - command.elbow_c[0] = limit_rate_position( - MAX_ELBOW_VELOCITY, - MAX_ELBOW_ACCELERATION, - MAX_ELBOW_JERK, - command.elbow_c[0], - robot_state.elbow_c[0], - robot_state.delbow_c[0], - robot_state.ddelbow_c[0], - ); - } - CartesianPose::check_elbow(&command.elbow_c); - } else { - command.valid_elbow = false; - command.elbow_c = [0.; 2]; - } - } -} +// impl ConvertMotion for CartesianVelocities { +// +// fn convert_motion( +// &self, +// robot_state: &FR3State, +// command: &mut MotionGeneratorCommand, +// cutoff_frequency: f64, +// limit_rate: bool, +// ) { +// command.O_dP_EE_c = self.O_dP_EE; +// if cutoff_frequency < MAX_CUTOFF_FREQUENCY { +// for i in 0..6 { +// command.O_dP_EE_c[i] = low_pass_filter( +// DELTA_T, +// command.O_dP_EE_c[i], +// robot_state.O_dP_EE_c[i], +// cutoff_frequency, +// ); +// } +// } +// 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, +// &command.O_dP_EE_c, +// &robot_state.O_dP_EE_c, +// &robot_state.O_ddP_EE_c, +// ); +// } +// command +// .O_dP_EE_c +// .iter() +// .for_each(|x| assert!(x.is_finite())); +// +// if self.has_elbow() { +// command.valid_elbow = true; +// command.elbow_c = self.elbow.unwrap(); +// if cutoff_frequency < MAX_CUTOFF_FREQUENCY { +// command.elbow_c[0] = low_pass_filter( +// DELTA_T, +// command.elbow_c[0], +// robot_state.elbow_c[0], +// cutoff_frequency, +// ); +// } +// if limit_rate { +// command.elbow_c[0] = limit_rate_position( +// MAX_ELBOW_VELOCITY, +// MAX_ELBOW_ACCELERATION, +// MAX_ELBOW_JERK, +// command.elbow_c[0], +// robot_state.elbow_c[0], +// robot_state.delbow_c[0], +// robot_state.ddelbow_c[0], +// ); +// } +// CartesianPose::check_elbow(&command.elbow_c); +// } else { +// command.valid_elbow = false; +// command.elbow_c = [0.; 2]; +// } +// } +// } impl MotionGeneratorTrait for CartesianVelocities { fn get_motion_generator_mode() -> MoveMotionGeneratorMode { diff --git a/src/robot/logger.rs b/src/robot/logger.rs index 5ce601b..7725a8d 100644 --- a/src/robot/logger.rs +++ b/src/robot/logger.rs @@ -33,7 +33,7 @@ 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: State, /// Robot command of timestamp n, after rate limiting (if activated). @@ -47,7 +47,7 @@ impl Record { } } -pub(crate) struct Logger { +pub(crate) struct Logger { states: VecDeque, commands: VecDeque, ring_front: usize, @@ -55,7 +55,7 @@ pub(crate) struct Logger { log_size: usize, } -impl Logger { +impl Logger { pub fn new(log_size: usize) -> Self { Logger { states: VecDeque::with_capacity(log_size), diff --git a/src/robot/robot_control.rs b/src/robot/robot_control.rs index 15020eb..b846359 100644 --- a/src/robot/robot_control.rs +++ b/src/robot/robot_control.rs @@ -7,7 +7,7 @@ use crate::robot::service_types::{MoveControllerMode, MoveDeviation, MoveMotionG use crate::robot::types::{ControllerCommand, MotionGeneratorCommand}; pub trait RobotControl { - type State2:RobotState; + type State2: RobotState; fn start_motion( &mut self, controller_mode: MoveControllerMode, diff --git a/src/robot/robot_impl.rs b/src/robot/robot_impl.rs index 93018ab..9c7efd8 100644 --- a/src/robot/robot_impl.rs +++ b/src/robot/robot_impl.rs @@ -14,33 +14,44 @@ use crate::robot::logger::{Logger, Record}; use crate::robot::robot_control::RobotControl; use crate::robot::robot_state::{FR3State, PandaState, RobotState}; use crate::robot::service_types::{ - ConnectRequest, ConnectRequestWithHeader, ConnectResponse, ConnectStatus, MoveControllerMode, - MoveDeviation, MoveMotionGeneratorMode, MoveRequest, MoveRequestWithHeader, MoveResponse, - MoveStatus, PandaCommandEnum, PandaCommandHeader, StopMoveRequestWithHeader, StopMoveResponse, - StopMoveStatus, VERSION, + ConnectRequest, ConnectRequestWithPandaHeader, ConnectResponsePanda, + ConnectResponseWithoutHeader, ConnectStatus, MoveControllerMode, MoveDeviation, + MoveMotionGeneratorMode, MoveRequest, MoveRequestWithPandaHeader, MoveResponse, + MoveStatusPanda, PandaCommandEnum, PandaCommandHeader, StopMoveRequestWithPandaHeader, + StopMoveStatusPanda, FR3_VERSION, PANDA_VERSION, }; -use crate::robot::types::{ControllerCommand, ControllerMode, RobotStateIntern, MotionGeneratorCommand, MotionGeneratorMode, PandaStateIntern, RobotCommand, RobotMode}; +use crate::robot::types::{ + ControllerCommand, ControllerMode, MotionGeneratorCommand, MotionGeneratorMode, + PandaStateIntern, RobotCommand, RobotMode, RobotStateIntern, +}; +use crate::RobotModel; use std::fs::remove_file; use std::path::Path; -use crate::RobotModel; -pub trait RobotImplementation: RobotControl::Data as RobotData>::State> { +pub trait RobotImplementation: + RobotControl::Data as RobotData>::State> +{ type Data: RobotData; fn update2( &mut self, motion_command: Option<&MotionGeneratorCommand>, control_command: Option<&ControllerCommand>, - ) -> FrankaResult<<::Data as RobotData>::State>; // todo remove commented out code - // { - // let robot_command = self.send_robot_command(motion_command, control_command)?; - // let state = Self::State::from(self.receive_robot_state()?); - // if let Some(command) = robot_command { - // self.logger.log(&state, &command); - // } - // Ok(state) - // } - fn read_once2(&mut self) -> FrankaResult<<::Data as RobotData>::State>; - fn load_model2(&mut self, persistent: bool) -> FrankaResult<<::Data as RobotData>::Model>; + ) -> FrankaResult<<::Data as RobotData>::State>; // todo remove commented out code + // { + // let robot_command = self.send_robot_command(motion_command, control_command)?; + // let state = Self::State::from(self.receive_robot_state()?); + // if let Some(command) = robot_command { + // self.logger.log(&state, &command); + // } + // Ok(state) + // } + fn read_once2( + &mut self, + ) -> FrankaResult<<::Data as RobotData>::State>; + fn load_model2( + &mut self, + persistent: bool, + ) -> FrankaResult<<::Data as RobotData>::Model>; fn server_version2(&self) -> u16; } @@ -92,7 +103,7 @@ impl RobotControl for RobotImplGeneric { { 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 { @@ -140,21 +151,22 @@ impl RobotControl for RobotImplGeneric { 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.get_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, + status, &robot_state.get_last_motion_errors(), self.logger.flush(), )); @@ -214,12 +226,12 @@ impl RobotControl for RobotImplGeneric { || 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, + status, &robot_state.get_last_motion_errors(), self.logger.flush(), )), @@ -230,10 +242,16 @@ impl RobotControl for RobotImplGeneric { } } -impl> RobotImplementation for RobotImplGeneric { +impl> RobotImplementation + for RobotImplGeneric +{ type Data = Data; - fn update2(&mut self, motion_command: Option<&MotionGeneratorCommand>, control_command: Option<&ControllerCommand>) -> FrankaResult<<::Data as RobotData>::State> { + fn update2( + &mut self, + motion_command: Option<&MotionGeneratorCommand>, + control_command: Option<&ControllerCommand>, + ) -> FrankaResult<<::Data as RobotData>::State> { let robot_command = self.send_robot_command(motion_command, control_command)?; let state = Data::State::from(self.receive_robot_state()?); if let Some(command) = robot_command { @@ -242,11 +260,13 @@ impl> RobotImplementation for Robo Ok(state) } - fn read_once2(&mut self) -> FrankaResult<<::Data as RobotData>::State> { + fn read_once2( + &mut self, + ) -> FrankaResult<<::Data as RobotData>::State> { while self.network.udp_receive::().is_some() {} Ok(Data::State::from(self.receive_robot_state()?)) } - fn load_model2(&mut self, persistent: bool) -> FrankaResult{ + fn load_model2(&mut self, persistent: bool) -> FrankaResult { let model_file = Path::new("/tmp/model.so"); let model_already_downloaded = model_file.exists(); if !model_already_downloaded { @@ -259,67 +279,46 @@ impl> RobotImplementation for Robo Ok(model) } - fn server_version2(&self) -> u16 { self.ri_version.unwrap() } } -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!", - )), - } -} - - +// fn handle_command_move_status(status: &MoveStatusPanda) -> 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!", +// )), +// } +// } -impl RobotImplGeneric { +impl RobotImplGeneric { pub fn new( network: Network, log_size: usize, @@ -349,17 +348,11 @@ impl RobotImplGeneric { } fn connect_robot(&mut self) -> Result<(), FrankaException> { - let connect_command = ConnectRequestWithHeader { - header: PandaData::create_header( - &mut self.network.command_id, - PandaCommandEnum::Connect, - size_of::(), - ), - request: ConnectRequest::new(self.network.get_udp_port()), - }; + 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: ConnectResponse = - self.network.tcp_blocking_receive_response(command_id); + 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); @@ -367,7 +360,7 @@ impl RobotImplGeneric { } _ => Err(FrankaException::IncompatibleLibraryVersionError { server_version: connect_response.version, - library_version: VERSION, + library_version: PANDA_VERSION, }), } } @@ -386,9 +379,9 @@ impl RobotImplGeneric { 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, - } + Some(s) => s.get_message_id(), + None => self.message_id, + } { latest_accepted_state = Some(received_state.unwrap()); } @@ -472,56 +465,55 @@ impl RobotImplGeneric { maximum_path_deviation: &MoveDeviation, maximum_goal_deviation: &MoveDeviation, ) -> FrankaResult { - let connect_command = MoveRequestWithHeader { - header: self.network.create_header_for_panda( - PandaCommandEnum::Move, - size_of::(), - ), - request: MoveRequest::new( + // let connect_command = MoveRequestWithPandaHeader { + // header: self.network.create_header_for_panda( + // PandaCommandEnum::Move, + // size_of::(), + // ), + // request: MoveRequest::new( + // *controller_mode, + // *motion_generator_mode, + // *maximum_path_deviation, + // *maximum_goal_deviation, + // ), + // }; + 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(connect_command); - let response: MoveResponse = self.network.tcp_blocking_receive_response(command_id); - handle_command_response_move(&response)?; + ); + 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 = StopMoveRequestWithHeader { - header: self.network.create_header_for_panda( - PandaCommandEnum::StopMove, - size_of::(), - ), - }; + let command = Data::create_stop_request(&mut self.network.command_id); 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)?; + let status = self.network.tcp_blocking_receive_status(command_id); + Data::handle_command_stop_move_status(status)?; Ok(command_id) } } - - - - - // impl MotionGeneratorTrait for RobotImpl { // fn get_motion_generator_mode() -> MoveMotionGeneratorMode { // MoveMotionGeneratorMode::JointVelocity // } // } -fn create_control_exception( +fn create_control_exception_old( message: String, - move_status: &MoveStatus, + move_status: &MoveStatusPanda, reflex_reasons: &FrankaErrors, log: Vec>, ) -> FrankaException { let mut exception_string = String::from(&message); - if move_status == &MoveStatus::ReflexAborted { + if move_status == &MoveStatusPanda::ReflexAborted { exception_string += " "; exception_string += reflex_reasons.to_string().as_str(); if log.len() >= 2 { diff --git a/src/robot/robot_state.rs b/src/robot/robot_state.rs index fb24237..f9c0bc2 100644 --- a/src/robot/robot_state.rs +++ b/src/robot/robot_state.rs @@ -6,16 +6,17 @@ use std::fmt::Debug; use std::time::Duration; use crate::robot::errors::{FrankaErrorKind, FrankaErrors}; -use crate::robot::types::{RobotMode, PandaStateIntern}; +use crate::robot::types::{PandaStateIntern, RobotMode}; +use crate::robot::FR3; use nalgebra::{Matrix3, Vector3}; use serde::Serialize; -use crate::robot::FR3; -pub trait RobotState : Clone + Debug { +pub trait RobotState: Clone + Debug { fn get_time(&self) -> Duration; - fn get_tau_J_d(&self) -> [f64;7]; + 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 robot state. @@ -265,251 +266,8 @@ pub struct PandaState { pub time: Duration, } -#[derive(Debug, Clone, Default)] -#[allow(non_snake_case)] -pub struct FR3State { - /// ![^{O}T_{EE}](https://latex.codecogs.com/png.latex?^{O}T_{EE}) - /// - /// Measured end effector pose in base frame. - /// Pose is represented as a 4x4 matrix in column-major format. - pub O_T_EE: [f64; 16], - /// ![{^OT_{EE}}_{d}](http://latex.codecogs.com/png.latex?{^OT_{EE}}_{d}) - /// - /// Last desired end effector pose of motion generation in base frame. - /// Pose is represented as a 4x4 matrix in column-major format. - pub O_T_EE_d: [f64; 16], - /// ![^{F}T_{EE}](http://latex.codecogs.com/png.latex?^{F}T_{EE}) - /// - /// End effector frame pose in flange frame. - /// Pose is represented as a 4x4 matrix in column-major format. - /// # See - /// * [`F_T_NE`](`Self::F_T_NE`) - /// * [`NE_T_EE`](`Self::NE_T_EE`) - /// * [`Robot`](`crate::Robot`) for an explanation of the NE and EE frames. - pub F_T_EE: [f64; 16], - /// ![^{F}T_{NE}](https://latex.codecogs.com/png.latex?^{F}T_{NE}) - /// - /// Nominal end effector frame pose in flange frame. - /// Pose is represented as a 4x4 matrix in column-major format. - /// # See - /// * [`F_T_NE`](`Self::F_T_NE`) - /// * [`NE_T_EE`](`Self::NE_T_EE`) - /// * [`Robot`](`crate::Robot`) for an explanation of the NE and EE frames. - pub F_T_NE: [f64; 16], - /// ![^{NE}T_{EE}](https://latex.codecogs.com/png.latex?^{NE}T_{EE}) - /// - /// End effector frame pose in nominal end effector frame. - /// Pose is represented as a 4x4 matrix in column-major format. - /// # See - /// * [`F_T_NE`](`Self::F_T_NE`) - /// * [`NE_T_EE`](`Self::NE_T_EE`) - /// * [`Robot`](`crate::Robot`) for an explanation of the NE and EE frames. - pub NE_T_EE: [f64; 16], - /// ![^{EE}T_{K}](https://latex.codecogs.com/png.latex?^{EE}T_{K}) - /// - /// Stiffness frame pose in end effector frame. - /// Pose is represented as a 4x4 matrix in column-major format. - /// - /// See also [K-Frame](`crate::Robot#stiffness-frame-k`) - pub EE_T_K: [f64; 16], - /// ![m_{EE}](https://latex.codecogs.com/png.latex?m_{EE}) - /// - /// Configured mass of the end effector. - pub m_ee: f64, - /// ![I_{EE}](https://latex.codecogs.com/png.latex?I_{EE}) - /// - /// Configured rotational inertia matrix of the end effector load with respect to center of mass. - pub I_ee: [f64; 9], - /// ![^{F}x_{C_{EE}}](https://latex.codecogs.com/png.latex?^{F}x_{C_{EE}}) - /// - /// Configured center of mass of the end effector load with respect to flange frame. - pub F_x_Cee: [f64; 3], - /// ![m_{load}](https://latex.codecogs.com/png.latex?m_{load}) - /// - /// Configured mass of the external load. - pub m_load: f64, - /// ![I_{load}](https://latex.codecogs.com/png.latex?I_{load}) - /// - /// Configured rotational inertia matrix of the external load with respect to center of mass. - pub I_load: [f64; 9], - /// ![^{F}x_{C_{load}}](https://latex.codecogs.com/png.latex?^{F}x_{C_{load}}) - /// - /// Configured center of mass of the external load with respect to flange frame. - pub F_x_Cload: [f64; 3], - /// ![m_{total}](https://latex.codecogs.com/png.latex?m_{total}) - /// - /// Sum of the mass of the end effector and the external load. - pub m_total: f64, - /// ![I_{total}](https://latex.codecogs.com/png.latex?I_{total}) - /// - /// Combined rotational inertia matrix of the end effector load and the external load with respect - /// to the center of mass. - pub I_total: [f64; 9], - /// ![^{F}x_{C_{total}}](https://latex.codecogs.com/png.latex?^{F}x_{C_{total}}) - /// - /// Combined center of mass of the end effector load and the external load with respect to flange - /// frame. - pub F_x_Ctotal: [f64; 3], - /// Elbow configuration. - /// - /// The values of the array are: - /// - \[0\] Position of the 3rd joint in \[rad\]. - /// - \[1\] Sign of the 4th joint. Can be +1 or -1. - pub elbow: [f64; 2], - /// Desired elbow configuration. - /// - /// The values of the array are: - /// - \[0\] Position of the 3rd joint in \[rad\]. - /// - \[1\] Sign of the 4th joint. Can be +1 or -1. - pub elbow_d: [f64; 2], - /// Commanded elbow configuration. - /// - /// The values of the array are: - /// - \[0\] Position of the 3rd joint in \[rad\]. - /// - \[1\] Sign of the 4th joint. Can be +1 or -1. - pub elbow_c: [f64; 2], - /// Commanded elbow velocity. - /// - /// The values of the array are: - /// - \[0\] Velocity of the 3rd joint in \[rad/s\]. - /// - \[1\] Sign of the 4th joint. Can be +1 or -1. - pub delbow_c: [f64; 2], - /// Commanded elbow acceleration. - /// - /// The values of the array are: - /// - \[0\] Acceleration of the 3rd joint in \[rad/s^2\]. - /// - \[1\] Sign of the 4th joint. Can be +1 or -1. - pub ddelbow_c: [f64; 2], - /// ![\tau_{J}](https://latex.codecogs.com/png.latex?\tau_{J}) - /// - /// Measured link-side joint torque sensor signals. Unit: \[Nm\] - pub tau_J: [f64; 7], - /// ![{\tau_J}_d](https://latex.codecogs.com/png.latex?{\tau_J}_d) - /// - /// Desired link-side joint torque sensor signals without gravity. Unit: \[Nm\] - pub tau_J_d: [f64; 7], - /// ![\dot{\tau_{J}}](https://latex.codecogs.com/png.latex?\dot{\tau_{J}}) - /// - /// Derivative of measured link-side joint torque sensor signals. Unit: [Nm/s] - pub dtau_J: [f64; 7], - /// ![q](https://latex.codecogs.com/png.latex?q) - /// - /// Measured joint position. Unit: \[rad\] - pub q: [f64; 7], - /// ![q_d](https://latex.codecogs.com/png.latex?q_d) - /// - /// Desired joint position. Unit: \[rad\] - pub q_d: [f64; 7], - /// ![\dot{q}](https://latex.codecogs.com/png.latex?\dot{q}) - /// - /// Measured joint velocity. Unit: \[rad/s\] - pub dq: [f64; 7], - /// ![\dot{q}_d](https://latex.codecogs.com/png.latex?\dot{q}_d) - /// - /// Desired joint velocity. Unit: \[rad/s\] - pub dq_d: [f64; 7], - /// ![\ddot{q}_d](https://latex.codecogs.com/png.latex?\ddot{q}_d) - /// - /// Desired joint acceleration. Unit: \[rad/s^2\] - pub ddq_d: [f64; 7], - /// Indicates which contact level is activated in which joint. After contact disappears, value - /// turns to zero. - /// - /// See [`Robot::set_collision_behavior`](`crate::Robot::set_collision_behavior`) - /// for setting sensitivity values. - pub joint_contact: [f64; 7], - /// Indicates which contact level is activated in which Cartesian dimension (x,y,z,R,P,Y). - /// After contact disappears, the value turns to zero. - /// - /// See [`Robot::set_collision_behavior`](`crate::Robot::set_collision_behavior`) - /// for setting sensitivity values. - pub cartesian_contact: [f64; 6], - /// Indicates which contact level is activated in which joint. After contact disappears, the value - /// stays the same until a reset command is sent. - /// - /// See [`Robot::set_collision_behavior`](`crate::Robot::set_collision_behavior`) - /// for setting sensitivity values. - /// - /// See [`Robot::automatic_error_recovery`](`crate::Robot::automatic_error_recovery`) - /// for performing a reset after a collision. - pub joint_collision: [f64; 7], - /// Indicates which contact level is activated in which Cartesian dimension (x,y,z,R,P,Y). - /// After contact disappears, the value stays the same until a reset command is sent. - /// - /// See [`Robot::set_collision_behavior`](`crate::Robot::set_collision_behavior`) - /// for setting sensitivity values. - /// - /// See [`Robot::automatic_error_recovery`](`crate::Robot::automatic_error_recovery`) - /// for performing a reset after a collision. - pub cartesian_collision: [f64; 6], - /// ![\hat{\tau}_{\text{ext}}](https://latex.codecogs.com/png.latex?\hat{\tau}_{\text{ext}}) - /// - /// External torque, filtered. Unit: \[Nm\] - pub tau_ext_hat_filtered: [f64; 7], - /// ![^OF_{K,\text{ext}}](https://latex.codecogs.com/png.latex?^OF_{K,\text{ext}}) - /// - /// Estimated external wrench (force, torque) acting on stiffness frame, expressed - /// relative to the base frame. See also @ref k-frame "K frame". - /// Unit: \[N,N,N,Nm,Nm,Nm\]. - pub O_F_ext_hat_K: [f64; 6], - /// ![^{K}F_{K,\text{ext}}](https://latex.codecogs.com/png.latex?^{K}F_{K,\text{ext}}) - /// - /// Estimated external wrench (force, torque) acting on stiffness frame, - /// expressed relative to the stiffness frame. See also @ref k-frame "K frame". - /// Unit: \[N,N,N,Nm,Nm,Nm\]. - pub K_F_ext_hat_K: [f64; 6], - /// ![{^OdP_{EE}}_{d}](https://latex.codecogs.com/png.latex?{^OdP_{EE}}_{d}) - /// - /// Desired end effector twist in base frame. - /// Unit: [m/s,m/s,m/s,rad/s,rad/s,rad/s] - pub O_dP_EE_d: [f64; 6], - /// ![{^OddP_O}](https://latex.codecogs.com/png.latex?{^OddP_O) - /// - /// Linear component of the acceleration of the robot's base, expressed in frame parallel to the - /// base frame, i.e. the base's translational acceleration. If the base is resting - /// this shows the direction of the gravity vector. It is hardcoded for now to `{0, 0, -9.81}`. - pub O_ddP_O: [f64; 3], - /// ![{^OT_{EE}}_{c}](https://latex.codecogs.com/png.latex?{^OT_{EE}}_{c}) - /// - /// Last commanded end effector pose of motion generation in base frame. - /// Pose is represented as a 4x4 matrix in column-major format. - pub O_T_EE_c: [f64; 16], - /// ![{^OdP_{EE}}_{c}](https://latex.codecogs.com/png.latex?{^OdP_{EE}}_{c}) - /// - /// Last commanded end effector twist in base frame. - /// Unit: [m/s,m/s,m/s,rad/s,rad/s,rad/s] - pub O_dP_EE_c: [f64; 6], - ///![{^OddP_{EE}}_{c}](https://latex.codecogs.com/png.latex?{^OddP_{EE}}_{c}) - /// - /// Last commanded end effector acceleration in base frame. - /// Unit: [m/s^2,m/s^2,m/s^2,rad/s^2,rad/s^2,rad/s^2] - pub O_ddP_EE_c: [f64; 6], - /// ![\theta](https://latex.codecogs.com/png.latex?\theta) - /// - /// Motor position. Unit: \[rad\] - pub theta: [f64; 7], - /// ![\dot{\theta}](https://latex.codecogs.com/png.latex?\dot{\theta}) - /// - /// Motor velocity. Unit: \[rad/s\] - pub dtheta: [f64; 7], - /// Current error state. - pub current_errors: FrankaErrors, - /// Contains the errors that aborted the previous motion - pub last_motion_errors: FrankaErrors, - /// Percentage of the last 100 control commands that were successfully received by the robot. - /// - /// Shows a value of zero if no control or motion generator loop is currently running. - /// - /// Range \[0,1\] - pub control_command_success_rate: f64, - /// Current robot mode. - pub robot_mode: RobotMode, - /// Strictly monotonically increasing timestamp since robot start. - /// - /// Inside of control loops "time_step" parameter of Robot::control can be used - /// instead - pub time: Duration, -} +pub type FR3State = PandaState; + impl From for PandaState { #[allow(non_snake_case)] fn from(robot_state: PandaStateIntern) -> Self { @@ -627,123 +385,6 @@ impl From for PandaState { } } -impl From for FR3State { - #[allow(non_snake_case)] - 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; - let NE_T_EE = robot_state.NE_T_EE; - let F_T_EE = robot_state.F_T_EE; - let EE_T_K = robot_state.EE_T_K; - let m_ee = robot_state.m_ee; - let F_x_Cee = robot_state.F_x_Cee; - let I_ee = robot_state.I_ee; - let m_load = robot_state.m_load; - let F_x_Cload = robot_state.F_x_Cload; - let I_load = robot_state.I_load; - let m_total = robot_state.m_ee + robot_state.m_load; - let F_x_Ctotal = combine_center_of_mass( - robot_state.m_ee, - robot_state.F_x_Cee, - robot_state.m_load, - robot_state.F_x_Cload, - ); - let I_total = combine_inertia_tensor( - robot_state.m_ee, - robot_state.F_x_Cee, - robot_state.I_ee, - robot_state.m_load, - robot_state.F_x_Cload, - robot_state.I_load, - m_total, - F_x_Ctotal, - ); - let elbow = robot_state.elbow; - let elbow_d = robot_state.elbow_d; - let elbow_c = robot_state.elbow_c; - let delbow_c = robot_state.delbow_c; - let ddelbow_c = robot_state.ddelbow_c; - let tau_J = robot_state.tau_J; - let tau_J_d = robot_state.tau_J_d; - let dtau_J = robot_state.dtau_J; - let q = robot_state.q; - let dq = robot_state.dq; - let q_d = robot_state.q_d; - let dq_d = robot_state.dq_d; - let ddq_d = robot_state.ddq_d; - let joint_contact = robot_state.joint_contact; - let cartesian_contact = robot_state.cartesian_contact; - let joint_collision = robot_state.joint_collision; - let cartesian_collision = robot_state.cartesian_collision; - let tau_ext_hat_filtered = robot_state.tau_ext_hat_filtered; - let O_F_ext_hat_K = robot_state.O_F_ext_hat_K; - let K_F_ext_hat_K = robot_state.K_F_ext_hat_K; - let O_dP_EE_d = robot_state.O_dP_EE_d; - let O_ddP_O = robot_state.O_ddP_O; - let O_T_EE_c = robot_state.O_T_EE_c; - let O_dP_EE_c = robot_state.O_dP_EE_c; - let O_ddP_EE_c = robot_state.O_ddP_EE_c; - let theta = robot_state.theta; - let dtheta = robot_state.dtheta; - let control_command_success_rate = robot_state.control_command_success_rate; - let time = Duration::from_millis(robot_state.message_id); - let robot_mode = robot_state.robot_mode; - let current_errors = FrankaErrors::new(robot_state.errors, FrankaErrorKind::Error); - let last_motion_errors = - FrankaErrors::new(robot_state.errors, FrankaErrorKind::ReflexReason); - FR3State { - O_T_EE, - O_T_EE_d, - F_T_EE, - F_T_NE, - NE_T_EE, - EE_T_K, - m_ee, - I_ee, - F_x_Cee, - m_load, - I_load, - F_x_Cload, - m_total, - I_total, - F_x_Ctotal, - elbow, - elbow_d, - elbow_c, - delbow_c, - ddelbow_c, - tau_J, - tau_J_d, - dtau_J, - q, - q_d, - dq, - dq_d, - ddq_d, - joint_contact, - cartesian_contact, - joint_collision, - cartesian_collision, - tau_ext_hat_filtered, - O_F_ext_hat_K, - K_F_ext_hat_K, - O_dP_EE_d, - O_ddP_O, - O_T_EE_c, - O_dP_EE_c, - O_ddP_EE_c, - theta, - dtheta, - current_errors, - last_motion_errors, - control_command_success_rate, - robot_mode, - time, - } - } -} - #[allow(non_snake_case, clippy::too_many_arguments)] fn combine_inertia_tensor( m_ee: f64, @@ -820,30 +461,16 @@ impl RobotState for PandaState { self.tau_J_d } - fn get_last_motion_errors(&self) -> & FrankaErrors { + fn get_last_motion_errors(&self) -> &FrankaErrors { &self.last_motion_errors } fn is_moving(&self) -> bool { self.robot_mode == RobotMode::Move } -} -impl RobotState for FR3State { - 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 } } diff --git a/src/robot/service_types.rs b/src/robot/service_types.rs index ff5c491..2525d0b 100644 --- a/src/robot/service_types.rs +++ b/src/robot/service_types.rs @@ -1,35 +1,94 @@ // Copyright (c) 2021 Marco Boneberger // Licensed under the EUPL-1.2-or-later +macro_rules! make_printer { + ($($element: ident: $ty: ty),*) => { + struct Foo { $($element: $ty),* } + } +} +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::gripper::types::CommandHeader; 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 { - -} +make_printer!(x: i32, y: String); +// define_request_with_header!(i32); -impl RobotHeader for PandaCommandHeader{ +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 + self.command_id } } -impl RobotHeader for FR3CommandHeader { - -} +impl RobotHeader for FR3CommandHeader {} impl CommandHeader for FR3CommandHeader { fn get_command_id(&self) -> u32 { @@ -51,8 +110,6 @@ impl CommandHeader for PandaCommandHeader { } } - - #[derive(Serialize_repr, Deserialize_repr, Debug, Copy, Clone)] #[repr(u32)] pub enum PandaCommandEnum { @@ -89,8 +146,6 @@ pub enum FR3CommandEnum { LoadModelLibrary, } - - #[derive(Serialize_repr, Deserialize_repr, Debug, Copy, Clone)] #[repr(u8)] pub enum DefaultStatus { @@ -107,7 +162,7 @@ pub enum ConnectStatus { #[derive(Serialize_repr, Deserialize_repr, Debug, Copy, Clone, PartialEq)] #[repr(u8)] -pub enum MoveStatus { +pub enum MoveStatusPanda { Success, MotionStarted, Preempted, @@ -120,11 +175,39 @@ 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 StopMoveStatusPanda { + Success, + CommandNotPossibleRejected, + EmergencyAborted, + ReflexAborted, + Aborted, +} + #[derive(Serialize_repr, Deserialize_repr, Debug, Copy, Clone)] #[repr(u8)] -pub enum StopMoveStatus { +pub enum StopMoveStatusFR3 { Success, CommandNotPossibleRejected, + CommandRejectedDueToActivatedSafetyFunctions, EmergencyAborted, ReflexAborted, Aborted, @@ -132,7 +215,7 @@ pub enum StopMoveStatus { #[derive(Serialize_repr, Deserialize_repr, Debug, Copy, Clone)] #[repr(u8)] -pub enum AutomaticErrorRecoveryStatus { +pub enum AutomaticErrorRecoveryStatusPanda { Success, CommandNotPossibleRejected, ManualErrorRecoveryRequiredRejected, @@ -141,6 +224,18 @@ pub enum AutomaticErrorRecoveryStatus { Aborted, } +#[derive(Serialize_repr, Deserialize_repr, Debug, Copy, Clone)] +#[repr(u8)] +pub enum AutomaticErrorRecoveryStatusFR3 { + Success, + CommandNotPossibleRejected, + CommandRejectedDueToActivatedSafetyFunctions, + ManualErrorRecoveryRequiredRejected, + ReflexAborted, + EmergencyAborted, + Aborted, +} + #[derive(Serialize_repr, Deserialize_repr, Debug, Copy, Clone)] #[repr(u8)] pub enum LoadModelLibraryStatus { @@ -150,10 +245,19 @@ 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)] @@ -164,30 +268,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)] +define_panda_request_with_header!( + ConnectRequestWithPandaHeader, + ConnectRequest, + PandaCommandEnum::Connect +); +define_fr3_request_with_header!( + ConnectRequestWithFR3Header, + ConnectRequest, + FR3CommandEnum::Connect +); + +#[derive(Serialize, Deserialize)] #[repr(packed)] -pub struct ConnectRequestWithHeader { +pub struct ConnectResponsePanda { pub header: PandaCommandHeader, - pub request: ConnectRequest, -} - -impl MessageCommand for ConnectRequestWithHeader { - fn get_command_message_id(&self) -> u32 { - self.header.get_command_message_id() - } + pub status: ConnectStatus, + pub version: u16, } -#[derive(Serialize, Deserialize, Debug)] -pub struct ConnectResponse { - pub header: PandaCommandHeader, +#[derive(Serialize, Deserialize)] +#[repr(packed)] +pub struct ConnectResponseWithoutHeader { pub status: ConnectStatus, pub version: u16, } @@ -242,42 +349,42 @@ impl MoveRequest { } } -#[derive(Serialize, Deserialize, Debug, Copy, Clone)] -#[repr(packed)] -pub struct MoveRequestWithHeader { - pub header: PandaCommandHeader, - pub request: MoveRequest, -} - -impl MessageCommand for MoveRequestWithHeader { - fn get_command_message_id(&self) -> u32 { - self.header.get_command_message_id() - } -} +define_panda_request_with_header!( + MoveRequestWithPandaHeader, + MoveRequest, + PandaCommandEnum::Move +); +define_fr3_request_with_header!(MoveRequestWithFR3Header, MoveRequest, FR3CommandEnum::Move); #[derive(Serialize, Deserialize, Debug)] pub struct MoveResponse { pub header: PandaCommandHeader, - pub status: MoveStatus, + pub status: MoveStatusPanda, } #[derive(Serialize, Deserialize, Debug, Copy, Clone)] #[repr(packed)] -pub struct StopMoveRequestWithHeader { +pub struct StopMoveRequestWithPandaHeader { pub header: PandaCommandHeader, } -impl MessageCommand for StopMoveRequestWithHeader { +#[derive(Serialize, Deserialize, Debug, Copy, Clone)] +#[repr(packed)] +pub struct StopMoveRequestWithFR3Header { + pub header: FR3CommandHeader, +} + +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: PandaCommandHeader, - pub status: StopMoveStatus, -} +// #[derive(Serialize, Deserialize, Debug)] +// pub struct StopMoveResponse { +// pub header: PandaCommandHeader, +// pub status: StopMoveStatus, +// } #[derive(Serialize, Deserialize, Debug, Copy, Clone)] #[repr(packed)] @@ -290,24 +397,16 @@ impl GetCartesianLimitRequest { GetCartesianLimitRequest { id } } } - -#[derive(Serialize, Deserialize, Debug, Copy, Clone)] -#[repr(packed)] -pub struct GetCartesianLimitRequestWithHeader { - pub header: PandaCommandHeader, - 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: PandaCommandHeader, - pub status: GetterSetterStatus, + pub status: GetterSetterStatusPanda, pub object_world_size: [f64; 3], pub object_frame: [f64; 16], pub object_activation: bool, @@ -350,21 +449,16 @@ impl SetCollisionBehaviorRequest { } } } - -#[derive(Serialize, Deserialize, Debug, Copy, Clone)] -#[repr(packed)] -pub struct SetCollisionBehaviorRequestWithHeader { - pub header: PandaCommandHeader, - 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)] @@ -380,19 +474,16 @@ impl SetJointImpedanceRequest { } } -#[derive(Serialize, Deserialize, Debug, Copy, Clone)] -#[repr(packed)] -pub struct SetJointImpedanceRequestWithHeader { - pub header: PandaCommandHeader, - pub request: SetJointImpedanceRequest, -} - -impl MessageCommand for SetJointImpedanceRequestWithHeader { - fn get_command_message_id(&self) -> u32 { - self.header.get_command_message_id() - } -} - +define_panda_request_with_header!( + SetJointImpedanceRequestWithPandaHeader, + SetJointImpedanceRequest, + PandaCommandEnum::SetJointImpedance +); +define_fr3_request_with_header!( + SetJointImpedanceRequestWithFR3Header, + SetJointImpedanceRequest, + FR3CommandEnum::SetJointImpedance +); pub type SetJointImpedanceResponse = SetterResponse; #[derive(Serialize, Deserialize, Debug, Copy, Clone)] @@ -409,18 +500,16 @@ impl SetCartesianImpedanceRequest { } } -#[derive(Serialize, Deserialize, Debug, Copy, Clone)] -#[repr(packed)] -pub struct SetCartesianImpedanceRequestWithHeader { - pub header: PandaCommandHeader, - pub request: SetCartesianImpedanceRequest, -} - -impl MessageCommand for SetCartesianImpedanceRequestWithHeader { - fn get_command_message_id(&self) -> u32 { - self.header.get_command_message_id() - } -} +define_panda_request_with_header!( + SetCartesianImpedanceRequestWithPandaHeader, + SetCartesianImpedanceRequest, + PandaCommandEnum::SetCartesianImpedance +); +define_fr3_request_with_header!( + SetCartesianImpedanceRequestWithFR3Header, + SetCartesianImpedanceRequest, + FR3CommandEnum::SetCartesianImpedance +); pub type SetCartesianImpedanceResponse = SetterResponse; @@ -442,18 +531,16 @@ impl SetGuidingModeRequest { } } -#[derive(Serialize, Deserialize, Debug, Copy, Clone)] -#[repr(packed)] -pub struct SetGuidingModeRequestWithHeader { - pub header: PandaCommandHeader, - pub request: SetGuidingModeRequest, -} - -impl MessageCommand for SetGuidingModeRequestWithHeader { - fn get_command_message_id(&self) -> u32 { - self.header.get_command_message_id() - } -} +define_panda_request_with_header!( + SetGuidingModeRequestWithPandaHeader, + SetGuidingModeRequest, + PandaCommandEnum::SetGuidingMode +); +define_fr3_request_with_header!( + SetGuidingModeRequestWithFR3Header, + SetGuidingModeRequest, + FR3CommandEnum::SetGuidingMode +); pub type SetGuidingModeResponse = SetterResponse; @@ -471,18 +558,16 @@ impl SetEeToKRequest { } } -#[derive(Serialize, Deserialize, Debug, Copy, Clone)] -#[repr(packed)] -pub struct SetEeToKRequestWithHeader { - pub header: PandaCommandHeader, - pub request: SetEeToKRequest, -} - -impl MessageCommand for SetEeToKRequestWithHeader { - fn get_command_message_id(&self) -> u32 { - self.header.get_command_message_id() - } -} +define_panda_request_with_header!( + SetEeToKRequestWithPandaHeader, + SetEeToKRequest, + PandaCommandEnum::SetEeToK +); +define_fr3_request_with_header!( + SetEeToKRequestWithFR3Header, + SetEeToKRequest, + FR3CommandEnum::SetEeToK +); pub type SetEeToKResponse = SetterResponse; @@ -500,18 +585,16 @@ impl SetNeToEeRequest { } } -#[derive(Serialize, Deserialize, Debug, Copy, Clone)] -#[repr(packed)] -pub struct SetNeToEeRequestWithHeader { - pub header: PandaCommandHeader, - pub request: SetNeToEeRequest, -} - -impl MessageCommand for SetNeToEeRequestWithHeader { - fn get_command_message_id(&self) -> u32 { - self.header.get_command_message_id() - } -} +define_panda_request_with_header!( + SetNeToEeRequestWithPandaHeader, + SetNeToEeRequest, + PandaCommandEnum::SetNeToEe +); +define_fr3_request_with_header!( + SetNeToEeRequestWithFR3Header, + SetNeToEeRequest, + FR3CommandEnum::SetNeToEe +); pub type SetNeToEeResponse = SetterResponse; @@ -535,20 +618,16 @@ impl SetLoadRequest { } } -#[derive(Serialize, Deserialize, Debug, Copy, Clone)] -#[repr(packed)] -pub struct SetLoadRequestWithHeader { - pub header: PandaCommandHeader, - 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)] @@ -580,35 +659,34 @@ impl SetFiltersRequest { } } -#[derive(Serialize, Deserialize, Debug, Copy, Clone)] -#[repr(packed)] -pub struct SetFiltersRequestWithHeader { - pub header: PandaCommandHeader, - 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: PandaCommandHeader, - pub status: GetterSetterStatus, + pub status: GetterSetterStatusPanda, } -pub type AutomaticErrorRecoveryRequestWithHeader = PandaCommandHeader; - #[derive(Serialize, Deserialize, Debug)] -pub struct AutomaticErrorRecoveryResponse { - pub header: PandaCommandHeader, - pub status: AutomaticErrorRecoveryStatus, +pub struct SetterResponseFR3 { + pub header: FR3CommandHeader, + pub status: GetterSetterStatusPanda, } +pub type AutomaticErrorRecoveryRequestWithPandaHeader = PandaCommandHeader; +pub type AutomaticErrorRecoveryRequestWithFR3Header = FR3CommandHeader; +// #[derive(Serialize, Deserialize, Debug)] +// pub struct AutomaticErrorRecoveryResponse { +// pub header: PandaCommandHeader, +// pub status: AutomaticErrorRecoveryStatusPanda, +// } + #[derive(Serialize_repr, Deserialize_repr, Debug, Copy, Clone)] #[repr(u8)] pub enum LoadModelLibraryArchitecture { @@ -632,32 +710,71 @@ pub struct LoadModelLibraryRequest { pub system: LoadModelLibrarySystem, } -#[derive(Serialize, Deserialize, Debug, Copy, Clone)] -#[repr(packed)] -pub struct LoadModelLibraryRequestWithHeader { - pub header: PandaCommandHeader, - pub request: LoadModelLibraryRequest, -} - -// this is a very hacky generic struct that is a generic version of LoadModelLibraryRequestWithHeader -// todo find a better solution to deal with the model downloader -#[derive(Serialize, Debug, Copy, Clone)] -pub struct ModelRequestWithHeader { - pub header: Header, - pub request: LoadModelLibraryRequest, -} - -impl MessageCommand for ModelRequestWithHeader
{ - fn get_command_message_id(&self) -> u32 { - self.header.get_command_message_id() - } -} - -impl MessageCommand for LoadModelLibraryRequestWithHeader { - fn get_command_message_id(&self) -> u32 { - self.header.get_command_message_id() - } -} +// #[derive(Serialize, Deserialize, Debug, Copy, Clone)] +// #[repr(packed)] +// pub struct LoadModelLibraryRequestWithHeader { +// pub header: PandaCommandHeader, +// pub request: LoadModelLibraryRequest, +// } + +// // this is a very hacky generic struct that is a generic version of LoadModelLibraryRequestWithHeader +// // todo find a better solution to deal with the model downloader +// #[derive(Serialize, Deserialize, Debug, Copy, Clone)] +// pub struct ModelRequestWithHeader { +// pub header: Header, +// pub request: LoadModelLibraryRequest, +// } +// +// impl MessageCommand for ModelRequestWithHeader
{ +// fn get_command_message_id(&self) -> u32 { +// self.header.get_command_message_id() +// } +// } + +define_panda_request_with_header!( + LoadModelLibraryRequestWithPandaHeader, + LoadModelLibraryRequest, + PandaCommandEnum::LoadModelLibrary +); +define_fr3_request_with_header!( + LoadModelLibraryRequestWithFR3Header, + LoadModelLibraryRequest, + FR3CommandEnum::LoadModelLibrary +); +// +// impl MessageCommand for LoadModelLibraryRequestWithHeader { +// fn get_command_message_id(&self) -> u32 { +// self.header.get_command_message_id() +// } +// } + +// impl From<(u32, LoadModelLibraryRequest)> for LoadModelLibraryRequestWithPandaHeader { +// fn from(tuple: (u32, LoadModelLibraryRequest)) -> Self { +// let command_id = tuple.0; +// LoadModelLibraryRequestWithPandaHeader { +// header: PandaCommandHeader::new( +// PandaCommandEnum::LoadModelLibrary, +// command_id, +// std::mem::size_of::() as u32, +// ), +// request: tuple.1, +// } +// } +// } + +// impl From<(u32, LoadModelLibraryRequest)> for LoadModelLibraryRequestWithFR3Header { +// fn from(tuple: (u32, LoadModelLibraryRequest)) -> Self { +// let command_id = tuple.0; +// LoadModelLibraryRequestWithFR3Header { +// header: FR3CommandHeader::new( +// FR3CommandEnum::LoadModelLibrary, +// command_id, +// std::mem::size_of::() as u32, +// ), +// request: tuple.1, +// } +// } +// } #[derive(Serialize, Deserialize, Debug)] pub struct LoadModelLibraryResponse { @@ -673,13 +790,12 @@ pub struct PandaCommandHeader { pub size: u32, } - impl Default for PandaCommandHeader { - fn default() -> Self { - PandaCommandHeader{ + fn default() -> Self { + PandaCommandHeader { command: PandaCommandEnum::Connect, command_id: 0, - size: 0 + size: 0, } } } diff --git a/src/robot/types.rs b/src/robot/types.rs index 22f39da..271d66c 100644 --- a/src/robot/types.rs +++ b/src/robot/types.rs @@ -2,11 +2,11 @@ // Licensed under the EUPL-1.2-or-later use std::fmt::Debug; +use crate::network::MessageCommand; use crate::robot::types::RobotMode::Other; use serde::Deserialize; use serde::Serialize; use serde_repr::{Deserialize_repr, Serialize_repr}; -use crate::network::MessageCommand; #[derive(Serialize_repr, Deserialize_repr, Debug, Copy, Clone, PartialEq)] #[repr(u8)] @@ -97,13 +97,15 @@ pub struct PandaStateIntern { pub control_command_success_rate: f64, } -pub trait RobotStateIntern : Copy + Clone + PartialEq{ +pub type FR3StateIntern = PandaStateIntern; + +pub trait RobotStateIntern: Copy + Clone + PartialEq { fn get_message_id(&self) -> u64; fn get_motion_generator_mode(&self) -> MotionGeneratorMode; fn get_controller_mode(&self) -> ControllerMode; } -impl RobotStateIntern for PandaStateIntern{ +impl RobotStateIntern for PandaStateIntern { fn get_message_id(&self) -> u64 { self.message_id } @@ -113,7 +115,7 @@ impl RobotStateIntern for PandaStateIntern{ } fn get_controller_mode(&self) -> ControllerMode { - self.controller_mode + self.controller_mode } } diff --git a/src/utils.rs b/src/utils.rs index 5d38d21..4c38117 100644 --- a/src/utils.rs +++ b/src/utils.rs @@ -3,10 +3,10 @@ //! contains useful type definitions and conversion functions. use crate::robot::control_types::{ConvertMotion, JointPositions}; -use crate::robot::robot_state::PandaState; +use crate::robot::robot_state::{PandaState, RobotState}; +use crate::Finishable; use nalgebra::{Isometry3, Matrix4, MatrixMN, MatrixN, Rotation3, Vector3, VectorN, U6, U7}; use std::time::Duration; -use crate::Finishable; /// converts a 4x4 column-major homogenous matrix to an Isometry pub fn array_to_isometry(array: &[f64; 16]) -> Isometry3 { @@ -170,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: &PandaState, + 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(); } From 6debb6fe55d98dfa98cbceb33fe107f9b5db7400 Mon Sep 17 00:00:00 2001 From: Marco Boneberger Date: Wed, 21 Dec 2022 14:53:51 +0100 Subject: [PATCH 07/63] fixup! implement requestwithheaders for panda and fr3 --- Cargo.toml | 2 +- examples/communication_test.rs | 1 - examples/echo_robot_state.rs | 2 +- examples/generate_cartesian_pose_motion.rs | 10 +- src/gripper.rs | 30 +++--- src/gripper/types.rs | 4 +- src/model.rs | 8 +- src/model/library_downloader.rs | 8 +- src/network.rs | 26 +++-- src/robot.rs | 41 ++++---- src/robot/control_loop.rs | 5 +- src/robot/control_types.rs | 2 +- src/robot/logger.rs | 2 +- src/robot/robot_control.rs | 8 +- src/robot/robot_impl.rs | 106 ++++----------------- src/robot/robot_state.rs | 3 +- src/robot/service_types.rs | 25 ----- src/robot/types.rs | 1 - src/utils.rs | 6 +- 19 files changed, 95 insertions(+), 195 deletions(-) diff --git a/Cargo.toml b/Cargo.toml index b410ec4..98048fd 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -68,7 +68,7 @@ name = "print_joint_poses" path = "examples/print_joint_poses.rs" [profile.dev] -opt-level = 0 +opt-level = 3 [dependencies] serde = { version = "1.0", features = ["derive"] } diff --git a/examples/communication_test.rs b/examples/communication_test.rs index ad82744..da2722e 100644 --- a/examples/communication_test.rs +++ b/examples/communication_test.rs @@ -1,7 +1,6 @@ // Copyright (c) 2021 Marco Boneberger // Licensed under the EUPL-1.2-or-later use clap::Parser; -use franka::robot::robot_state::FR3State; use franka::robot::{Robot, FR3}; use franka::Torques; use franka::{Finishable, FrankaResult, PandaState}; diff --git a/examples/echo_robot_state.rs b/examples/echo_robot_state.rs index de54df0..9dde7df 100644 --- a/examples/echo_robot_state.rs +++ b/examples/echo_robot_state.rs @@ -3,7 +3,7 @@ use clap::Parser; use franka::robot::{Robot, FR3}; -use franka::{FrankaResult, Panda, PandaState}; +use franka::{FrankaResult, PandaState}; // use franka::Robot; // use franka::RobotState; diff --git a/examples/generate_cartesian_pose_motion.rs b/examples/generate_cartesian_pose_motion.rs index 203b8a1..5351127 100644 --- a/examples/generate_cartesian_pose_motion.rs +++ b/examples/generate_cartesian_pose_motion.rs @@ -2,12 +2,10 @@ // Licensed under the EUPL-1.2-or-later use clap::Parser; -use franka::{ConvertMotion, Finishable, FrankaResult, Panda}; -// use franka::Robot; use franka::robot::robot_state::FR3State; use franka::robot::{Robot, FR3}; use franka::CartesianPose; -use franka::PandaState; +use franka::{Finishable, FrankaResult}; use std::f64::consts::PI; use std::time::Duration; @@ -23,7 +21,7 @@ struct CommandLineArguments { fn main() -> FrankaResult<()> { let address = CommandLineArguments::parse(); - let mut robot = Panda::new(address.franka_ip.as_str(), None, None)?; + let mut robot = FR3::new(address.franka_ip.as_str(), 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..."); @@ -42,11 +40,11 @@ fn main() -> FrankaResult<()> { )?; let q_goal = [0., -PI / 4., 0., -3. * PI / 4., 0., PI / 2., PI / 4.]; - // robot.joint_motion(0.5, &q_goal)?; + robot.joint_motion(0.5, &q_goal)?; println!("Finished moving to initial joint configuration."); let mut initial_pose = CartesianPose::new([0.0; 16], None); let mut time = 0.; - let callback = |state: &PandaState, time_step: &Duration| -> CartesianPose { + let callback = |state: &FR3State, time_step: &Duration| -> CartesianPose { time += time_step.as_secs_f64(); if time == 0. { initial_pose.O_T_EE = state.O_T_EE_c; diff --git a/src/gripper.rs b/src/gripper.rs index 8cf12c9..5e974a4 100644 --- a/src/gripper.rs +++ b/src/gripper.rs @@ -10,9 +10,9 @@ use crate::gripper::gripper_state::GripperState; use crate::gripper::types::{ ConnectRequest, ConnectRequestWithHeader, ConnectResponse, GraspRequest, GraspRequestWithHeader, GripperCommandEnum, GripperCommandHeader, GripperStateIntern, - MoveRequest, MoveRequestWithHeader, Status, COMMAND_PORT, VERSION, + MoveRequest, MoveRequestWithHeader, Status, COMMAND_PORT, GRIPPER_VERSION, }; -use crate::network::{GripperData, Network, NetworkType}; +use crate::network::{GripperData, Network}; pub mod gripper_state; pub(crate) mod types; @@ -33,14 +33,14 @@ 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) -> FrankaResult<()> { @@ -198,7 +198,7 @@ mod tests { use crate::gripper::types::{ ConnectRequestWithHeader, ConnectResponse, GraspRequest, GraspRequestWithHeader, GripperCommandEnum, GripperCommandHeader, GripperStateIntern, MoveRequest, - MoveRequestWithHeader, Status, COMMAND_PORT, VERSION, + MoveRequestWithHeader, Status, COMMAND_PORT, GRIPPER_VERSION, }; use crate::gripper::Gripper; use crate::FrankaResult; @@ -408,7 +408,7 @@ mod tests { let requests_server = requests.clone(); let thread = std::thread::spawn(|| { - let mut mock_gripper = GripperMockServer::new(VERSION); + let mut mock_gripper = GripperMockServer::new(GRIPPER_VERSION); let mut mock = MockServerReaction::default(); let num_requests = requests_server.len(); let mut counter = 0; @@ -441,7 +441,7 @@ mod tests { { 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(); } @@ -453,7 +453,7 @@ 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 mut mock = MockServerReaction::default(); mock.expect_process_received_bytes() .returning(move |bytes: &mut Vec| -> Vec { @@ -491,7 +491,7 @@ 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 mut mock = MockServerReaction::default(); mock.expect_process_received_bytes() .returning(move |bytes: &mut Vec| -> Vec { @@ -553,7 +553,7 @@ mod tests { let requests_server = requests.clone(); let thread = std::thread::spawn(|| { - let mut mock_gripper = GripperMockServer::new(VERSION); + let mut mock_gripper = GripperMockServer::new(GRIPPER_VERSION); let mut mock = MockServerReaction::default(); let num_requests = requests_server.len(); let mut counter = 0; @@ -605,7 +605,7 @@ 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 mut mock = MockServerReaction::default(); mock.expect_process_received_bytes() .returning(|_bytes| Vec::::new()); @@ -634,7 +634,7 @@ 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 mut mock = MockServerReaction::default(); mock.expect_process_received_bytes() .returning(|_bytes| Vec::::new()); diff --git a/src/gripper/types.rs b/src/gripper/types.rs index 4510fa6..ad6d1ed 100644 --- a/src/gripper/types.rs +++ b/src/gripper/types.rs @@ -11,7 +11,7 @@ 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)] @@ -114,7 +114,7 @@ pub struct GraspRequest { impl ConnectRequest { pub fn new(udp_port: u16) -> Self { ConnectRequest { - version: VERSION, + version: GRIPPER_VERSION, udp_port, } } diff --git a/src/model.rs b/src/model.rs index 0f55177..15dee3e 100644 --- a/src/model.rs +++ b/src/model.rs @@ -69,7 +69,7 @@ pub trait RobotModel { fn new>(model_filename: S, libm_filename: Option<&Path>) -> FrankaResult where Self: Sized; - + #[allow(non_snake_case)] fn pose( &self, frame: &Frame, @@ -79,6 +79,7 @@ pub trait RobotModel { ) -> [f64; 16]; fn pose_from_state(&self, frame: &Frame, robot_state: &Self::State) -> [f64; 16]; + #[allow(non_snake_case)] fn body_jacobian( &self, frame: &Frame, @@ -88,6 +89,7 @@ pub trait RobotModel { ) -> [f64; 42]; fn body_jacobian_from_state(&self, frame: &Frame, robot_state: &Self::State) -> [f64; 42]; + #[allow(non_snake_case)] fn zero_jacobian( &self, frame: &Frame, @@ -96,6 +98,7 @@ pub trait RobotModel { EE_T_K: &[f64; 16], ) -> [f64; 42]; fn zero_jacobian_from_state(&self, frame: &Frame, robot_state: &Self::State) -> [f64; 42]; + #[allow(non_snake_case)] fn mass( &self, q: &[f64; 7], @@ -104,6 +107,7 @@ pub trait RobotModel { F_x_Ctotal: &[f64; 3], ) -> [f64; 49]; fn mass_from_state(&self, robot_state: &Self::State) -> [f64; 49]; + #[allow(non_snake_case)] fn coriolis( &self, q: &[f64; 7], @@ -113,6 +117,7 @@ pub trait RobotModel { F_x_Ctotal: &[f64; 3], ) -> [f64; 7]; fn coriolis_from_state(&self, robot_state: &Self::State) -> [f64; 7]; + #[allow(non_snake_case)] fn gravity<'a, Grav: Into>>( &self, q: &[f64; 7], @@ -120,6 +125,7 @@ pub trait RobotModel { F_x_Ctotal: &[f64; 3], gravity_earth: Grav, ) -> [f64; 7]; + #[allow(non_snake_case)] fn gravity_from_state<'a, Grav: Into>>( &self, robot_state: &Self::State, diff --git a/src/model/library_downloader.rs b/src/model/library_downloader.rs index 03c20d8..f2bab21 100644 --- a/src/model/library_downloader.rs +++ b/src/model/library_downloader.rs @@ -1,11 +1,10 @@ // Copyright (c) 2021 Marco Boneberger // Licensed under the EUPL-1.2-or-later use crate::exception::FrankaException::ModelException; -use crate::network::{DeviceData, FR3Data, Network, PandaData, RobotData}; +use crate::network::{Network, RobotData}; use crate::robot::service_types::{ - FR3CommandEnum, LoadModelLibraryArchitecture, LoadModelLibraryRequest, - LoadModelLibraryResponse, LoadModelLibrarySystem, PandaCommandEnum, PandaCommandHeader, - RobotHeader, + LoadModelLibraryArchitecture, LoadModelLibraryRequest, LoadModelLibraryResponse, + LoadModelLibrarySystem, }; use crate::FrankaResult; use std::fmt; @@ -13,7 +12,6 @@ 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 trait LibraryDownloader { diff --git a/src/network.rs b/src/network.rs index 71df68e..ed3e31b 100644 --- a/src/network.rs +++ b/src/network.rs @@ -24,10 +24,12 @@ use nix::sys::socket::setsockopt; use nix::sys::socket::sockopt::{KeepAlive, TcpKeepCount, TcpKeepIdle, TcpKeepInterval}; use serde::de::DeserializeOwned; -use serde::{Deserialize, Serialize}; +use serde::Serialize; use crate::exception::{create_command_exception, FrankaException, FrankaResult}; -use crate::gripper::types::{CommandHeader, GripperCommandEnum, GripperCommandHeader}; +use crate::gripper::types::{ + CommandHeader, GripperCommandEnum, GripperCommandHeader, GRIPPER_VERSION, +}; use crate::robot::errors::FrankaErrors; use crate::robot::logger::Record; use crate::robot::robot_state::{FR3State, RobotState}; @@ -68,6 +70,7 @@ pub trait DeviceData { command: Self::CommandEnum, size: usize, ) -> Self::CommandHeader; + fn get_library_version() -> u16; } pub trait RobotData: DeviceData { @@ -219,6 +222,10 @@ impl DeviceData for PandaData { *command_id += 1; header } + + fn get_library_version() -> u16 { + PANDA_VERSION + } } impl RobotData for PandaData { @@ -448,6 +455,10 @@ impl DeviceData for FR3Data { *command_id += 1; header } + + fn get_library_version() -> u16 { + FR3_VERSION + } } impl RobotData for FR3Data { type DeviceData = Self; @@ -701,6 +712,10 @@ impl DeviceData for GripperData { *command_id += 1; header } + + fn get_library_version() -> u16 { + GRIPPER_VERSION + } } pub trait MessageCommand { @@ -727,11 +742,7 @@ pub struct Network { } impl Network { - pub fn new( - network_type: NetworkType, - franka_address: &str, - franka_port: u16, - ) -> Result, Box> { + 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)?); @@ -1079,7 +1090,6 @@ fn deserialize(encoded: &[u8]) -> T { #[cfg(test)] mod tests { use crate::network::{deserialize, serialize}; - use crate::robot::service_types::PandaCommandHeader; use crate::robot::types::PandaStateIntern; #[test] diff --git a/src/robot.rs b/src/robot.rs index 2b9111a..a24b214 100644 --- a/src/robot.rs +++ b/src/robot.rs @@ -8,8 +8,7 @@ use std::time::Duration; use std::fmt::Debug; use crate::exception::{create_command_exception, FrankaException, FrankaResult}; -use crate::model::PandaModel; -use crate::network::{FR3Data, Network, NetworkType, PandaData, RobotData}; +use crate::network::{FR3Data, Network, PandaData, RobotData}; use crate::robot::control_loop::ControlLoop; use crate::robot::control_types::{ CartesianPose, CartesianVelocities, ControllerMode, ConvertMotion, JointPositions, @@ -19,7 +18,6 @@ use crate::robot::low_pass_filter::{DEFAULT_CUTOFF_FREQUENCY, MAX_CUTOFF_FREQUEN use crate::robot::motion_generator_traits::MotionGeneratorTrait; use crate::robot::robot_control::RobotControl; use crate::robot::robot_impl::{RobotImplGeneric, RobotImplementation}; -use crate::robot::robot_state::{FR3State, RobotState}; use crate::robot::service_types::{ GetCartesianLimitRequest, GetCartesianLimitRequestWithPandaHeader, GetCartesianLimitResponse, GetterSetterStatusPanda, PandaCommandEnum, SetCartesianImpedanceRequest, @@ -30,7 +28,6 @@ use crate::robot::service_types::{ use crate::robot::virtual_wall_cuboid::VirtualWallCuboid; use crate::utils::MotionGenerator; use crate::Finishable; -use robot_state::PandaState; mod control_loop; mod control_tools; @@ -87,7 +84,7 @@ where mut read_callback: F, ) -> FrankaResult<()> { loop { - let state = self.get_rob_mut().update2(None, None)?; + let state = self.get_rob_mut().update(None, None)?; if !read_callback(&state) { break; } @@ -103,7 +100,7 @@ where /// /// See [`Robot::read`](`Self::read`) for a way to repeatedly receive the robot state. fn read_once(&mut self) -> FrankaResult<<::Data as RobotData>::State> { - self.get_rob_mut().read_once2() + self.get_rob_mut().read_once() } /// Changes the collision behavior. /// @@ -801,7 +798,7 @@ where /// * [`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. fn load_model(&mut self, persistent: bool) -> FrankaResult<::Model> { - self.get_rob_mut().load_model2(persistent) + self.get_rob_mut().load_model(persistent) } /// Sets a default collision behavior, joint impedance and Cartesian impedance. @@ -836,7 +833,7 @@ where /// # Return /// Software version of the connected server. fn server_version(&self) -> u16 { - self.get_rob().server_version2() + self.get_rob().server_version() } } @@ -939,13 +936,10 @@ impl FR3 { ) -> 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::FR3, - franka_address, - service_types::COMMAND_PORT, - ) - .map_err(|_| FrankaException::NetworkException { - message: "Connection could not be established".to_string(), + let network = Network::new(franka_address, service_types::COMMAND_PORT).map_err(|_| { + FrankaException::NetworkException { + message: "Connection could not be established".to_string(), + } })?; Ok(FR3 { robimpl: ::Rob::new(network, log_size, realtime_config)?, @@ -983,13 +977,10 @@ impl Panda { ) -> 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::Panda, - franka_address, - service_types::COMMAND_PORT, - ) - .map_err(|_| FrankaException::NetworkException { - message: "Connection could not be established".to_string(), + let network = Network::new(franka_address, service_types::COMMAND_PORT).map_err(|_| { + FrankaException::NetworkException { + message: "Connection could not be established".to_string(), + } })?; Ok(Panda { robimpl: ::Rob::new(network, log_size, realtime_config)?, @@ -1090,10 +1081,10 @@ mod tests { use crate::robot::service_types::{ ConnectRequestWithPandaHeader, ConnectResponsePanda, ConnectStatus, FR3CommandEnum, FR3CommandHeader, GetterSetterStatusPanda, MoveControllerMode, MoveDeviation, - MoveMotionGeneratorMode, MoveRequest, MoveRequestWithPandaHeader, MoveResponse, - MoveStatusPanda, PandaCommandEnum, PandaCommandHeader, SetCollisionBehaviorRequest, + MoveMotionGeneratorMode, MoveRequest, MoveRequestWithPandaHeader, MoveStatusPanda, + PandaCommandEnum, PandaCommandHeader, SetCollisionBehaviorRequest, SetCollisionBehaviorRequestWithFR3Header, SetCollisionBehaviorRequestWithPandaHeader, - SetterResponseFR3, COMMAND_PORT, FR3_VERSION, PANDA_VERSION, + SetterResponseFR3, COMMAND_PORT, FR3_VERSION, }; use crate::robot::types::PandaStateIntern; use crate::robot::{Robot, FR3}; diff --git a/src/robot/control_loop.rs b/src/robot/control_loop.rs index defbe77..d9abe72 100644 --- a/src/robot/control_loop.rs +++ b/src/robot/control_loop.rs @@ -12,9 +12,8 @@ use crate::robot::control_types::{ControllerMode, ConvertMotion, RealtimeConfig, 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_impl::RobotImplementation; -use crate::robot::robot_state::{PandaState, RobotState}; +use crate::robot::robot_state::RobotState; use crate::robot::service_types::{MoveControllerMode, MoveDeviation}; use crate::robot::types::{ControllerCommand, MotionGeneratorCommand}; use crate::Finishable; @@ -222,7 +221,7 @@ impl< } fn spin_motion( &mut self, - robot_state: &T::State2, + robot_state: &T::State, time_step: &Duration, command: &mut MotionGeneratorCommand, ) -> bool { diff --git a/src/robot/control_types.rs b/src/robot/control_types.rs index 90d5b13..f64393f 100644 --- a/src/robot/control_types.rs +++ b/src/robot/control_types.rs @@ -18,7 +18,7 @@ use crate::robot::rate_limiting::{ MAX_ROTATIONAL_ACCELERATION, MAX_ROTATIONAL_JERK, MAX_ROTATIONAL_VELOCITY, MAX_TRANSLATIONAL_ACCELERATION, MAX_TRANSLATIONAL_JERK, MAX_TRANSLATIONAL_VELOCITY, }; -use crate::robot::robot_state::{FR3State, PandaState, RobotState}; +use crate::robot::robot_state::{PandaState, RobotState}; use crate::robot::service_types::MoveMotionGeneratorMode; use crate::robot::types::MotionGeneratorCommand; use crate::utils::Vector7; diff --git a/src/robot/logger.rs b/src/robot/logger.rs index 7725a8d..ed71f3f 100644 --- a/src/robot/logger.rs +++ b/src/robot/logger.rs @@ -5,7 +5,7 @@ use crate::robot::control_types::{ CartesianPose, CartesianVelocities, JointPositions, JointVelocities, Torques, }; -use crate::robot::robot_state::{PandaState, RobotState}; +use crate::robot::robot_state::RobotState; use crate::robot::types::RobotCommand; use std::collections::VecDeque; use std::fmt::Debug; diff --git a/src/robot/robot_control.rs b/src/robot/robot_control.rs index b846359..e724bfc 100644 --- a/src/robot/robot_control.rs +++ b/src/robot/robot_control.rs @@ -2,12 +2,12 @@ // Licensed under the EUPL-1.2-or-later use crate::exception::FrankaResult; use crate::robot::control_types::RealtimeConfig; -use crate::robot::robot_state::{PandaState, RobotState}; +use crate::robot::robot_state::RobotState; use crate::robot::service_types::{MoveControllerMode, MoveDeviation, MoveMotionGeneratorMode}; use crate::robot::types::{ControllerCommand, MotionGeneratorCommand}; pub trait RobotControl { - type State2: RobotState; + type State: RobotState; fn start_motion( &mut self, controller_mode: MoveControllerMode, @@ -26,11 +26,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: &Self::State2, + robot_state: &Self::State, motion_id: u32, ) -> FrankaResult<()>; } diff --git a/src/robot/robot_impl.rs b/src/robot/robot_impl.rs index 9c7efd8..33f3a17 100644 --- a/src/robot/robot_impl.rs +++ b/src/robot/robot_impl.rs @@ -1,58 +1,37 @@ // 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::exception::{FrankaException, FrankaResult}; use crate::model::library_downloader::{LibraryDownloader, LibraryDownloaderGeneric}; -use crate::model::PandaModel; -use crate::network::DeviceData; -use crate::network::{FR3Data, Network, PandaData, RobotData}; +use crate::network::{Network, RobotData}; use crate::robot::control_types::RealtimeConfig; -use crate::robot::errors::FrankaErrors; -use crate::robot::logger::{Logger, Record}; +use crate::robot::logger::Logger; use crate::robot::robot_control::RobotControl; -use crate::robot::robot_state::{FR3State, PandaState, RobotState}; +use crate::robot::robot_state::{PandaState, RobotState}; use crate::robot::service_types::{ - ConnectRequest, ConnectRequestWithPandaHeader, ConnectResponsePanda, ConnectResponseWithoutHeader, ConnectStatus, MoveControllerMode, MoveDeviation, - MoveMotionGeneratorMode, MoveRequest, MoveRequestWithPandaHeader, MoveResponse, - MoveStatusPanda, PandaCommandEnum, PandaCommandHeader, StopMoveRequestWithPandaHeader, - StopMoveStatusPanda, FR3_VERSION, PANDA_VERSION, + MoveMotionGeneratorMode, MoveRequest, MoveResponse, }; use crate::robot::types::{ - ControllerCommand, ControllerMode, MotionGeneratorCommand, MotionGeneratorMode, - PandaStateIntern, RobotCommand, RobotMode, RobotStateIntern, + ControllerCommand, ControllerMode, MotionGeneratorCommand, MotionGeneratorMode, RobotCommand, + RobotStateIntern, }; use crate::RobotModel; use std::fs::remove_file; use std::path::Path; pub trait RobotImplementation: - RobotControl::Data as RobotData>::State> + RobotControl::Data as RobotData>::State> { type Data: RobotData; - fn update2( - &mut self, - motion_command: Option<&MotionGeneratorCommand>, - control_command: Option<&ControllerCommand>, - ) -> FrankaResult<<::Data as RobotData>::State>; // todo remove commented out code - // { - // let robot_command = self.send_robot_command(motion_command, control_command)?; - // let state = Self::State::from(self.receive_robot_state()?); - // if let Some(command) = robot_command { - // self.logger.log(&state, &command); - // } - // Ok(state) - // } - fn read_once2( + fn read_once( &mut self, ) -> FrankaResult<<::Data as RobotData>::State>; - fn load_model2( + fn load_model( &mut self, persistent: bool, ) -> FrankaResult<<::Data as RobotData>::Model>; - fn server_version2(&self) -> u16; + fn server_version(&self) -> u16; } pub struct RobotImplGeneric { @@ -68,7 +47,7 @@ pub struct RobotImplGeneric { } impl RobotControl for RobotImplGeneric { - type State2 = Data::State; + type State = Data::State; fn start_motion( &mut self, @@ -247,26 +226,13 @@ impl> RobotImplementation { type Data = Data; - fn update2( - &mut self, - motion_command: Option<&MotionGeneratorCommand>, - control_command: Option<&ControllerCommand>, - ) -> FrankaResult<<::Data as RobotData>::State> { - let robot_command = self.send_robot_command(motion_command, control_command)?; - let state = Data::State::from(self.receive_robot_state()?); - if let Some(command) = robot_command { - self.logger.log(&PandaState::default(), &command); // todo log properly - } - Ok(state) - } - - fn read_once2( + fn read_once( &mut self, ) -> FrankaResult<<::Data as RobotData>::State> { while self.network.udp_receive::().is_some() {} Ok(Data::State::from(self.receive_robot_state()?)) } - fn load_model2(&mut self, persistent: bool) -> FrankaResult { + 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 { @@ -279,7 +245,7 @@ impl> RobotImplementation Ok(model) } - fn server_version2(&self) -> u16 { + fn server_version(&self) -> u16 { self.ri_version.unwrap() } } @@ -360,7 +326,7 @@ impl RobotImplGeneric { } _ => Err(FrankaException::IncompatibleLibraryVersionError { server_version: connect_response.version, - library_version: PANDA_VERSION, + library_version: Data::get_library_version(), }), } } @@ -499,43 +465,3 @@ impl RobotImplGeneric { Ok(command_id) } } - -// impl MotionGeneratorTrait for RobotImpl { -// fn get_motion_generator_mode() -> MoveMotionGeneratorMode { -// MoveMotionGeneratorMode::JointVelocity -// } -// } - -fn create_control_exception_old( - message: String, - move_status: &MoveStatusPanda, - 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), - } -} diff --git a/src/robot/robot_state.rs b/src/robot/robot_state.rs index f9c0bc2..cd31d5c 100644 --- a/src/robot/robot_state.rs +++ b/src/robot/robot_state.rs @@ -7,12 +7,11 @@ use std::time::Duration; use crate::robot::errors::{FrankaErrorKind, FrankaErrors}; use crate::robot::types::{PandaStateIntern, RobotMode}; -use crate::robot::FR3; use nalgebra::{Matrix3, Vector3}; -use serde::Serialize; pub trait RobotState: 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; diff --git a/src/robot/service_types.rs b/src/robot/service_types.rs index 2525d0b..819c0ca 100644 --- a/src/robot/service_types.rs +++ b/src/robot/service_types.rs @@ -1,11 +1,6 @@ // Copyright (c) 2021 Marco Boneberger // Licensed under the EUPL-1.2-or-later -macro_rules! make_printer { - ($($element: ident: $ty: ty),*) => { - struct Foo { $($element: $ty),* } - } -} macro_rules! define_panda_request_with_header { ($name: ident, $request: ty, $command: expr) => { #[derive(Serialize, Deserialize, Debug, Copy, Clone)] @@ -75,9 +70,6 @@ pub static PANDA_VERSION: u16 = 5; pub static FR3_VERSION: u16 = 6; pub static COMMAND_PORT: u16 = 1337; -make_printer!(x: i32, y: String); -// define_request_with_header!(i32); - pub trait RobotHeader: MessageCommand + Serialize + Debug + Copy + Clone {} impl RobotHeader for PandaCommandHeader {} @@ -484,7 +476,6 @@ define_fr3_request_with_header!( SetJointImpedanceRequest, FR3CommandEnum::SetJointImpedance ); -pub type SetJointImpedanceResponse = SetterResponse; #[derive(Serialize, Deserialize, Debug, Copy, Clone)] #[repr(packed)] @@ -511,8 +502,6 @@ define_fr3_request_with_header!( FR3CommandEnum::SetCartesianImpedance ); -pub type SetCartesianImpedanceResponse = SetterResponse; - #[derive(Serialize, Deserialize, Debug, Copy, Clone)] #[repr(packed)] #[allow(non_snake_case)] @@ -542,8 +531,6 @@ define_fr3_request_with_header!( FR3CommandEnum::SetGuidingMode ); -pub type SetGuidingModeResponse = SetterResponse; - #[derive(Serialize, Deserialize, Debug, Copy, Clone)] #[repr(packed)] #[allow(non_snake_case)] @@ -569,8 +556,6 @@ define_fr3_request_with_header!( FR3CommandEnum::SetEeToK ); -pub type SetEeToKResponse = SetterResponse; - #[derive(Serialize, Deserialize, Debug, Copy, Clone)] #[repr(packed)] #[allow(non_snake_case)] @@ -596,8 +581,6 @@ define_fr3_request_with_header!( FR3CommandEnum::SetNeToEe ); -pub type SetNeToEeResponse = SetterResponse; - #[derive(Serialize, Deserialize, Debug, Copy, Clone)] #[repr(packed)] #[allow(non_snake_case)] @@ -679,14 +662,6 @@ pub struct SetterResponseFR3 { pub status: GetterSetterStatusPanda, } -pub type AutomaticErrorRecoveryRequestWithPandaHeader = PandaCommandHeader; -pub type AutomaticErrorRecoveryRequestWithFR3Header = FR3CommandHeader; -// #[derive(Serialize, Deserialize, Debug)] -// pub struct AutomaticErrorRecoveryResponse { -// pub header: PandaCommandHeader, -// pub status: AutomaticErrorRecoveryStatusPanda, -// } - #[derive(Serialize_repr, Deserialize_repr, Debug, Copy, Clone)] #[repr(u8)] pub enum LoadModelLibraryArchitecture { diff --git a/src/robot/types.rs b/src/robot/types.rs index 271d66c..77dfd56 100644 --- a/src/robot/types.rs +++ b/src/robot/types.rs @@ -2,7 +2,6 @@ // Licensed under the EUPL-1.2-or-later use std::fmt::Debug; -use crate::network::MessageCommand; use crate::robot::types::RobotMode::Other; use serde::Deserialize; use serde::Serialize; diff --git a/src/utils.rs b/src/utils.rs index 4c38117..63b8d55 100644 --- a/src/utils.rs +++ b/src/utils.rs @@ -2,8 +2,8 @@ // Licensed under the EUPL-1.2-or-later //! contains useful type definitions and conversion functions. -use crate::robot::control_types::{ConvertMotion, JointPositions}; -use crate::robot::robot_state::{PandaState, RobotState}; +use crate::robot::control_types::JointPositions; +use crate::robot::robot_state::RobotState; use crate::Finishable; use nalgebra::{Isometry3, Matrix4, MatrixMN, MatrixN, Rotation3, Vector3, VectorN, U6, U7}; use std::time::Duration; @@ -193,7 +193,7 @@ impl MotionGenerator { #[cfg(test)] mod test { - use crate::{array_to_isometry, ConvertMotion, Finishable, MotionGenerator, PandaState}; + use crate::{array_to_isometry, Finishable, MotionGenerator, PandaState}; use nalgebra::Rotation3; use std::time::Duration; From 76c67da06eb92a01acf589e721f9ae0152b95c81 Mon Sep 17 00:00:00 2001 From: Marco Boneberger Date: Wed, 21 Dec 2022 15:56:22 +0100 Subject: [PATCH 08/63] rename state3 generic to state --- src/robot/control_types.rs | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/robot/control_types.rs b/src/robot/control_types.rs index f64393f..06e8781 100644 --- a/src/robot/control_types.rs +++ b/src/robot/control_types.rs @@ -41,11 +41,11 @@ pub enum RealtimeConfig { /// Helper type for control and motion generation loops. /// /// Used to determine whether to terminate a loop after the control callback has returned. -pub trait ConvertMotion { +pub trait ConvertMotion { /// converts the motion type to a MotionGeneratorCommand and applies rate limiting and filtering fn convert_motion( &self, - robot_state: &State3, + robot_state: &State, command: &mut MotionGeneratorCommand, cutoff_frequency: f64, limit_rate: bool, From 024c4d4ce7398eabceab448fb7ede19908f301a6 Mon Sep 17 00:00:00 2001 From: Marco Boneberger Date: Wed, 21 Dec 2022 16:05:47 +0100 Subject: [PATCH 09/63] finalize generic logger --- src/network.rs | 12 ++++++------ src/robot/robot_impl.rs | 8 ++++---- 2 files changed, 10 insertions(+), 10 deletions(-) diff --git a/src/network.rs b/src/network.rs index ed3e31b..e7a33e3 100644 --- a/src/network.rs +++ b/src/network.rs @@ -187,13 +187,13 @@ pub trait RobotData: DeviceData { message: String, move_status: Self::MoveStatus, reflex_reasons: &FrankaErrors, - log: Vec>, + log: Vec>, ) -> FrankaException; fn create_control_exception_if_reflex_aborted( message: String, move_status: Self::MoveStatus, reflex_reasons: &FrankaErrors, - log: Vec>, + log: Vec>, ) -> FrankaResult<()>; fn handle_getter_setter_status(status: Self::GetterSetterStatus) -> FrankaResult<()>; @@ -313,7 +313,7 @@ impl RobotData for PandaData { message: String, move_status: Self::MoveStatus, reflex_reasons: &FrankaErrors, - log: Vec>, + log: Vec>, ) -> FrankaException { let mut exception_string = String::from(&message); if move_status == MoveStatusPanda::ReflexAborted { @@ -348,7 +348,7 @@ impl RobotData for PandaData { message: String, move_status: Self::MoveStatus, reflex_reasons: &FrankaErrors, - log: Vec>, + log: Vec>, ) -> FrankaResult<()> { // todo think about if option is a good return type if move_status == MoveStatusPanda::ReflexAborted { @@ -563,7 +563,7 @@ impl RobotData for FR3Data { message: String, move_status: Self::MoveStatus, reflex_reasons: &FrankaErrors, - log: Vec>, + log: Vec>, ) -> FrankaException { let mut exception_string = String::from(&message); if move_status == MoveStatusFR3::ReflexAborted { @@ -598,7 +598,7 @@ impl RobotData for FR3Data { message: String, move_status: Self::MoveStatus, reflex_reasons: &FrankaErrors, - log: Vec>, + log: Vec>, ) -> FrankaResult<()> { if move_status == MoveStatusFR3::ReflexAborted { return Err(Self::create_control_exception( diff --git a/src/robot/robot_impl.rs b/src/robot/robot_impl.rs index 33f3a17..9335554 100644 --- a/src/robot/robot_impl.rs +++ b/src/robot/robot_impl.rs @@ -7,7 +7,7 @@ use crate::robot::control_types::RealtimeConfig; use crate::robot::logger::Logger; use crate::robot::robot_control::RobotControl; -use crate::robot::robot_state::{PandaState, RobotState}; +use crate::robot::robot_state::RobotState; use crate::robot::service_types::{ ConnectResponseWithoutHeader, ConnectStatus, MoveControllerMode, MoveDeviation, MoveMotionGeneratorMode, MoveRequest, MoveResponse, @@ -36,7 +36,7 @@ pub trait RobotImplementation: pub struct RobotImplGeneric { pub network: Network, - logger: Logger, + logger: Logger, realtime_config: RealtimeConfig, ri_version: Option, motion_generator_mode: Option, @@ -187,7 +187,7 @@ impl RobotControl for RobotImplGeneric { let robot_command = self.send_robot_command(motion_command, control_command)?; let state = Data::State::from(self.receive_robot_state()?); if let Some(command) = robot_command { - self.logger.log(&PandaState::default(), &command); // todo log properly + self.logger.log(&state, &command); } Ok(state) } @@ -292,7 +292,7 @@ impl RobotImplGeneric { ) -> FrankaResult { let current_move_generator_mode = MotionGeneratorMode::Idle; let controller_mode = ControllerMode::Other; - let logger = Logger::new(log_size); + let logger: Logger = Logger::new(log_size); let mut robot_impl = RobotImplGeneric { network, logger, From fbcc69fcdf1c9cac95d8bc9c57242c6702511c56 Mon Sep 17 00:00:00 2001 From: Marco Boneberger Date: Thu, 22 Dec 2022 09:21:10 +0100 Subject: [PATCH 10/63] rename PandaState to RobotState and fix tcp_receive_response --- examples/cartesian_impedance_control.rs | 4 ++-- examples/communication_test.rs | 4 ++-- examples/echo_robot_state.rs | 13 +++++------ examples/generate_cartesian_pose_motion.rs | 7 +++--- .../generate_cartesian_velocity_motion.rs | 4 ++-- examples/generate_consecutive_motions.rs | 4 ++-- examples/generate_elbow_motion.rs | 4 ++-- examples/generate_joint_position_motion.rs | 4 ++-- examples/generate_joint_velocity_motion.rs | 4 ++-- examples/mirror_robot.rs | 4 ++-- src/exception.rs | 4 ++-- src/lib.rs | 14 ++++++------ src/model.rs | 8 +++---- src/network.rs | 20 ++++++++--------- src/robot.rs | 12 +++++----- src/robot/control_loop.rs | 2 +- src/robot/control_types.rs | 22 +++++++++---------- src/robot/logger.rs | 6 ++--- src/robot/robot_control.rs | 4 ++-- src/robot/robot_impl.rs | 10 ++++----- src/robot/robot_state.rs | 12 +++++----- src/robot/service_types.rs | 6 ----- src/robot/types.rs | 4 ++-- src/utils.rs | 8 +++---- 24 files changed, 87 insertions(+), 97 deletions(-) diff --git a/examples/cartesian_impedance_control.rs b/examples/cartesian_impedance_control.rs index f1765df..633ef8f 100644 --- a/examples/cartesian_impedance_control.rs +++ b/examples/cartesian_impedance_control.rs @@ -2,7 +2,7 @@ // Licensed under the EUPL-1.2-or-later use clap::Parser; -use franka::robot::robot_state::FR3State; +use franka::robot::robot_state::RobotState; use franka::robot::{Robot, FR3}; use franka::FrankaResult; use franka::Torques; @@ -66,7 +66,7 @@ fn main() -> FrankaResult<()> { println!("Press Enter to continue..."); std::io::stdin().read_line(&mut String::new()).unwrap(); let result = robot.control_torques( - |state: &FR3State, _step: &Duration| -> 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 jacobian = Matrix6x7::from_column_slice(&jacobian_array); diff --git a/examples/communication_test.rs b/examples/communication_test.rs index da2722e..bb5ae1e 100644 --- a/examples/communication_test.rs +++ b/examples/communication_test.rs @@ -3,7 +3,7 @@ use clap::Parser; use franka::robot::{Robot, FR3}; use franka::Torques; -use franka::{Finishable, FrankaResult, PandaState}; +use franka::{Finishable, FrankaResult, RobotState}; use std::f64::consts::PI; use std::time::Duration; @@ -46,7 +46,7 @@ fn main() -> FrankaResult<()> { let mut min_success_rate = 1.; let mut max_success_rate = 0.; - let callback = |state: &PandaState, time_step: &Duration| -> Torques { + let callback = |state: &RobotState, time_step: &Duration| -> Torques { time += time_step.as_millis() as u64; if time == 0 { return zero_torques; diff --git a/examples/echo_robot_state.rs b/examples/echo_robot_state.rs index 9dde7df..0122ae7 100644 --- a/examples/echo_robot_state.rs +++ b/examples/echo_robot_state.rs @@ -3,9 +3,7 @@ use clap::Parser; use franka::robot::{Robot, FR3}; -use franka::{FrankaResult, PandaState}; -// use franka::Robot; -// use franka::RobotState; +use franka::{FrankaResult, RobotState}; /// An example showing how to continuously read the robot state. #[derive(Parser, Debug)] @@ -17,17 +15,18 @@ struct CommandLineArguments { fn main() -> FrankaResult<()> { // let address = CommandLineArguments::parse(); - let mut robot = FR3::new("localhost", None, None)?; - // robot.set_collision_behavior([0.;7],[0.;7],[0.;7],[0.;7],[0.;6],[0.;6],[0.;6],[0.;6]); + let mut robot = FR3::new("172.116.0.5", None, None)?; + robot.set_collision_behavior( + [0.; 7], [0.; 7], [0.; 7], [0.; 7], [0.; 6], [0.; 6], [0.; 6], [0.; 6], + )?; robot.set_joint_impedance([3000., 3000., 3000., 2500., 2500., 2000., 2000.])?; let mut count = 0; - robot.read(|robot_state: &PandaState| { + robot.read(|robot_state: &RobotState| { // Printing to standard output adds a delay. This is acceptable for a read loop such as this, but // should not be done in a control loop. println!("{:?}", robot_state); count += 1; count <= 100 })?; - // println!("Done"); Ok(()) } diff --git a/examples/generate_cartesian_pose_motion.rs b/examples/generate_cartesian_pose_motion.rs index 5351127..c57c400 100644 --- a/examples/generate_cartesian_pose_motion.rs +++ b/examples/generate_cartesian_pose_motion.rs @@ -2,9 +2,8 @@ // Licensed under the EUPL-1.2-or-later use clap::Parser; -use franka::robot::robot_state::FR3State; use franka::robot::{Robot, FR3}; -use franka::CartesianPose; +use franka::{CartesianPose, RobotState}; use franka::{Finishable, FrankaResult}; use std::f64::consts::PI; use std::time::Duration; @@ -22,7 +21,7 @@ struct CommandLineArguments { fn main() -> FrankaResult<()> { let address = CommandLineArguments::parse(); let mut robot = FR3::new(address.franka_ip.as_str(), None, None)?; - // robot.set_default_behavior()?; + 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(); @@ -44,7 +43,7 @@ fn main() -> FrankaResult<()> { println!("Finished moving to initial joint configuration."); let mut initial_pose = CartesianPose::new([0.0; 16], None); let mut time = 0.; - let callback = |state: &FR3State, time_step: &Duration| -> CartesianPose { + let callback = |state: &RobotState, time_step: &Duration| -> CartesianPose { time += time_step.as_secs_f64(); if time == 0. { initial_pose.O_T_EE = state.O_T_EE_c; diff --git a/examples/generate_cartesian_velocity_motion.rs b/examples/generate_cartesian_velocity_motion.rs index bbe50b6..6af6abc 100644 --- a/examples/generate_cartesian_velocity_motion.rs +++ b/examples/generate_cartesian_velocity_motion.rs @@ -2,7 +2,7 @@ // Licensed under the EUPL-1.2-or-later use clap::Parser; -use franka::robot::robot_state::FR3State; +use franka::robot::robot_state::RobotState; use franka::robot::{Robot, FR3}; use franka::{CartesianVelocities, Finishable, FrankaResult}; use std::f64::consts::PI; @@ -56,7 +56,7 @@ fn main() -> FrankaResult<()> { let v_max = 0.1; let angle = PI / 4.; let mut time = 0.; - let callback = |_state: &FR3State, period: &Duration| -> CartesianVelocities { + let callback = |_state: &RobotState, period: &Duration| -> CartesianVelocities { time += period.as_secs_f64(); let cycle = f64::floor(f64::powf( diff --git a/examples/generate_consecutive_motions.rs b/examples/generate_consecutive_motions.rs index 0eb6721..4b98277 100644 --- a/examples/generate_consecutive_motions.rs +++ b/examples/generate_consecutive_motions.rs @@ -4,7 +4,7 @@ use clap::Parser; use franka::exception::FrankaException; use franka::robot::{Robot, FR3}; -use franka::{Finishable, FrankaResult, JointVelocities, PandaState}; +use franka::{Finishable, FrankaResult, JointVelocities, RobotState}; use std::f64::consts::PI; use std::time::Duration; @@ -46,7 +46,7 @@ fn main() -> FrankaResult<()> { let mut time = 0.; let omega_max = 0.2; let time_max = 4.0; - let callback = move |_state: &PandaState, time_step: &Duration| -> JointVelocities { + let callback = move |_state: &RobotState, time_step: &Duration| -> JointVelocities { time += time_step.as_secs_f64(); let cycle = f64::floor(f64::powf( diff --git a/examples/generate_elbow_motion.rs b/examples/generate_elbow_motion.rs index 32158e3..9ef9edc 100644 --- a/examples/generate_elbow_motion.rs +++ b/examples/generate_elbow_motion.rs @@ -3,7 +3,7 @@ use clap::Parser; use franka::robot::{Robot, FR3}; -use franka::{CartesianPose, Finishable, FrankaResult, PandaState}; +use franka::{CartesianPose, Finishable, FrankaResult, RobotState}; use std::f64::consts::PI; use std::time::Duration; @@ -43,7 +43,7 @@ fn main() -> FrankaResult<()> { let mut initial_pose = [0.; 16]; let mut initial_elbow = [0.; 2]; let mut time = 0.; - let callback = |state: &PandaState, time_step: &Duration| -> CartesianPose { + let callback = |state: &RobotState, time_step: &Duration| -> CartesianPose { time += time_step.as_secs_f64(); if time == 0. { initial_pose = state.O_T_EE_c; diff --git a/examples/generate_joint_position_motion.rs b/examples/generate_joint_position_motion.rs index 3ae36c9..dcca9ae 100644 --- a/examples/generate_joint_position_motion.rs +++ b/examples/generate_joint_position_motion.rs @@ -3,7 +3,7 @@ use clap::Parser; use franka::robot::{Robot, FR3}; -use franka::{Finishable, FrankaResult, JointPositions, PandaState}; +use franka::{Finishable, FrankaResult, JointPositions, RobotState}; use std::f64::consts::PI; use std::time::Duration; @@ -43,7 +43,7 @@ fn main() -> FrankaResult<()> { 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 time = 0.; - let callback = |state: &PandaState, time_step: &Duration| -> JointPositions { + let callback = |state: &RobotState, time_step: &Duration| -> JointPositions { time += time_step.as_secs_f64(); if time == 0. { initial_position.q = state.q_d; diff --git a/examples/generate_joint_velocity_motion.rs b/examples/generate_joint_velocity_motion.rs index 3ed34af..02738ab 100644 --- a/examples/generate_joint_velocity_motion.rs +++ b/examples/generate_joint_velocity_motion.rs @@ -4,7 +4,7 @@ use clap::Parser; use franka::robot::{Robot, FR3}; use franka::JointVelocities; -use franka::PandaState; +use franka::RobotState; use franka::{Finishable, FrankaResult}; use std::f64::consts::PI; use std::time::Duration; @@ -46,7 +46,7 @@ fn main() -> FrankaResult<()> { let mut time = 0.; let omega_max = 1.0; let time_max = 1.0; - let callback = move |_state: &PandaState, time_step: &Duration| -> JointVelocities { + let callback = move |_state: &RobotState, time_step: &Duration| -> JointVelocities { time += time_step.as_secs_f64(); let cycle = f64::floor(f64::powf( diff --git a/examples/mirror_robot.rs b/examples/mirror_robot.rs index 047209a..c814b79 100644 --- a/examples/mirror_robot.rs +++ b/examples/mirror_robot.rs @@ -7,7 +7,7 @@ use franka::model::Frame; use franka::robot::control_types::Torques; use franka::robot::{Robot, FR3}; use franka::utils::{array_to_isometry, Matrix6x7, Vector7}; -use franka::PandaState; +use franka::RobotState; use franka::{Matrix7, RobotModel}; use nalgebra::{Matrix3, Matrix6, Matrix6x1, Quaternion, UnitQuaternion, Vector3, U1, U3}; use std::sync::mpsc::channel; @@ -99,7 +99,7 @@ fn main() -> FrankaResult<()> { }); robot_mirror.control_torques( - |state: &PandaState, _step: &Duration| -> 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); diff --git a/src/exception.rs b/src/exception.rs index e3b9bd9..6892705 100644 --- a/src/exception.rs +++ b/src/exception.rs @@ -3,7 +3,7 @@ //! Contains exception and Result definitions use crate::robot::logger::Record; -use crate::PandaState; +use crate::RobotState; use thiserror::Error; /// Represents all kind of errors which correspond to the franka::Exception in the C++ version of @@ -16,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, }, diff --git a/src/lib.rs b/src/lib.rs index 4254954..8ceb2b2 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -23,7 +23,7 @@ //!```no_run //! use std::time::Duration; //! use std::f64::consts::PI; -//! use franka::{JointPositions, MotionFinished, PandaState, Panda, FrankaResult}; +//! use franka::{JointPositions, MotionFinished, RobotState, Panda, FrankaResult}; //! fn main() -> FrankaResult<()> { //! let mut robot = Panda::new("robotik-bs.de", None, None)?; //! robot.set_default_behavior()?; @@ -35,7 +35,7 @@ //! robot.joint_motion(0.5, &q_goal)?; //! let mut initial_position = JointPositions::new([0.0; 7]); //! let mut time = 0.; -//! let callback = |state: &PandaState, time_step: &Duration| -> JointPositions { +//! let callback = |state: &RobotState, time_step: &Duration| -> JointPositions { //! time += time_step.as_secs_f64(); //! if time == 0. { //! initial_position.q = state.q_d; @@ -60,7 +60,7 @@ //!```no_run //! # use std::time::Duration; //! # use std::f64::consts::PI; -//! # use franka::{JointPositions, MotionFinished, PandaState, Panda, FrankaResult}; +//! # use franka::{JointPositions, MotionFinished, RobotState, Panda, FrankaResult}; //! # fn main() -> FrankaResult<()> { //! let mut robot = Robot::new("robotik-bs.de", None, None)?; //! # Ok(()) @@ -80,7 +80,7 @@ //!```no_run //! # use std::time::Duration; //! # use std::f64::consts::PI; -//! # use franka::{JointPositions, MotionFinished, PandaState, Robot, FrankaResult}; +//! # use franka::{JointPositions, MotionFinished, RobotState, Robot, FrankaResult}; //! # fn main() -> FrankaResult<()> { //! # let mut robot = Robot::new("robotik-bs.de", None, None)?; //! let q_goal = [0., -PI / 4., 0., -3. * PI / 4., 0., PI / 2., PI / 4.]; @@ -124,8 +124,8 @@ //! 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::{Finishable, JointPositions}; //! # fn joint_positions() -> JointPositions { //! # let time = 0.; //! # let mut out = JointPositions::new([0.;7]); @@ -181,6 +181,6 @@ 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::PandaState; +pub use robot::robot_state::RobotState; pub use robot::Panda; pub use utils::*; diff --git a/src/model.rs b/src/model.rs index 15dee3e..0f66aee 100644 --- a/src/model.rs +++ b/src/model.rs @@ -5,7 +5,7 @@ use nalgebra::Matrix4; use crate::model::model_library::ModelLibrary; -use crate::robot::robot_state::{FR3State, PandaState, RobotState}; +use crate::robot::robot_state::{AbstractRobotState, RobotState}; use crate::FrankaResult; use std::fmt; use std::path::Path; @@ -65,7 +65,7 @@ impl fmt::Display for Frame { } pub trait RobotModel { - type State: RobotState; + type State: AbstractRobotState; fn new>(model_filename: S, libm_filename: Option<&Path>) -> FrankaResult where Self: Sized; @@ -140,7 +140,7 @@ pub struct PandaModel { #[allow(non_snake_case)] impl RobotModel for PandaModel { - type State = PandaState; + type State = RobotState; /// 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 @@ -469,7 +469,7 @@ pub struct FR3Model { #[allow(non_snake_case)] impl RobotModel for FR3Model { - type State = FR3State; + type State = RobotState; /// 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 diff --git a/src/network.rs b/src/network.rs index e7a33e3..adad706 100644 --- a/src/network.rs +++ b/src/network.rs @@ -32,7 +32,7 @@ use crate::gripper::types::{ }; use crate::robot::errors::FrankaErrors; use crate::robot::logger::Record; -use crate::robot::robot_state::{FR3State, RobotState}; +use crate::robot::robot_state::AbstractRobotState; use crate::robot::service_types::{ AutomaticErrorRecoveryStatusFR3, AutomaticErrorRecoveryStatusPanda, ConnectRequest, ConnectRequestWithFR3Header, ConnectRequestWithPandaHeader, FR3CommandEnum, FR3CommandHeader, @@ -51,8 +51,8 @@ use crate::robot::service_types::{ SetNeToEeRequestWithFR3Header, SetNeToEeRequestWithPandaHeader, StopMoveStatusFR3, StopMoveStatusPanda, FR3_VERSION, PANDA_VERSION, }; -use crate::robot::types::{FR3StateIntern, PandaStateIntern, RobotStateIntern}; -use crate::{FR3Model, PandaModel, PandaState, RobotModel}; +use crate::robot::types::{AbstractRobotStateIntern, FR3StateIntern, PandaStateIntern}; +use crate::{FR3Model, PandaModel, RobotModel, RobotState}; const CLIENT: Token = Token(1); @@ -76,8 +76,8 @@ pub trait DeviceData { pub trait RobotData: DeviceData { type DeviceData: DeviceData; type Header: RobotHeader; - type State: RobotState + From; - type StateIntern: Debug + DeserializeOwned + Serialize + RobotStateIntern + 'static; + type State: AbstractRobotState + From; + type StateIntern: Debug + DeserializeOwned + Serialize + AbstractRobotStateIntern + 'static; type Model: RobotModel; type LoadModelRequestWithHeader: MessageCommand + Serialize @@ -229,7 +229,7 @@ impl DeviceData for PandaData { } impl RobotData for PandaData { - type State = PandaState; + type State = RobotState; type DeviceData = Self; type StateIntern = PandaStateIntern; type Model = PandaModel; @@ -463,7 +463,7 @@ impl DeviceData for FR3Data { impl RobotData for FR3Data { type DeviceData = Self; type Header = FR3CommandHeader; - type State = FR3State; + type State = RobotState; type StateIntern = FR3StateIntern; type Model = FR3Model; type LoadModelRequestWithHeader = LoadModelLibraryRequestWithFR3Header; @@ -923,11 +923,11 @@ impl Network { 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); diff --git a/src/robot.rs b/src/robot.rs index a24b214..cff6e36 100644 --- a/src/robot.rs +++ b/src/robot.rs @@ -63,11 +63,11 @@ where /// /// This minimal example will print the robot state 100 times: /// ```no_run - /// use franka::{Panda, PandaState, FrankaResult}; + /// use franka::{Panda, RobotState, FrankaResult}; /// fn main() -> FrankaResult<()> { /// let mut robot = Panda::new("robotik-bs.de",None,None)?; /// let mut count = 0; - /// robot.read(| robot_state:&PandaState | -> bool { + /// robot.read(| robot_state:&RobotState | -> bool { /// println!("{:?}", robot_state); /// count += 1; /// count <= 100 @@ -872,12 +872,12 @@ where /// The following incomplete example shows the general structure of a callback function: /// /// ```no_run -/// use franka::robot::robot_state::PandaState; +/// use franka::robot::robot_state::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])} /// let mut time = 0.; -/// let callback = |state: &PandaState, time_step: &Duration| -> JointPositions { +/// let callback = |state: &RobotState, time_step: &Duration| -> JointPositions { /// time += time_step.as_secs_f64(); /// let out: JointPositions = your_function_which_generates_joint_positions(time); /// if time >= 5.0 { @@ -1088,7 +1088,7 @@ mod tests { }; use crate::robot::types::PandaStateIntern; use crate::robot::{Robot, FR3}; - use crate::{Finishable, FrankaResult, JointPositions, Panda, PandaState, RealtimeConfig}; + use crate::{Finishable, FrankaResult, JointPositions, Panda, RealtimeConfig, RobotState}; use bincode::{deserialize, serialize, serialized_size}; use std::iter::FromIterator; use std::mem::size_of; @@ -1491,7 +1491,7 @@ mod tests { let mut first_time = true; let mut start_counter = 0; robot - .read(|state: &PandaState| { + .read(|state: &RobotState| { if first_time { first_time = false; counter = state.time.as_millis(); diff --git a/src/robot/control_loop.rs b/src/robot/control_loop.rs index d9abe72..f4788ca 100644 --- a/src/robot/control_loop.rs +++ b/src/robot/control_loop.rs @@ -13,7 +13,7 @@ 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_impl::RobotImplementation; -use crate::robot::robot_state::RobotState; +use crate::robot::robot_state::AbstractRobotState; use crate::robot::service_types::{MoveControllerMode, MoveDeviation}; use crate::robot::types::{ControllerCommand, MotionGeneratorCommand}; use crate::Finishable; diff --git a/src/robot/control_types.rs b/src/robot/control_types.rs index 06e8781..86c31bf 100644 --- a/src/robot/control_types.rs +++ b/src/robot/control_types.rs @@ -18,7 +18,7 @@ use crate::robot::rate_limiting::{ MAX_ROTATIONAL_ACCELERATION, MAX_ROTATIONAL_JERK, MAX_ROTATIONAL_VELOCITY, MAX_TRANSLATIONAL_ACCELERATION, MAX_TRANSLATIONAL_JERK, MAX_TRANSLATIONAL_VELOCITY, }; -use crate::robot::robot_state::{PandaState, RobotState}; +use crate::robot::robot_state::{AbstractRobotState, RobotState}; use crate::robot::service_types::MoveMotionGeneratorMode; use crate::robot::types::MotionGeneratorCommand; use crate::utils::Vector7; @@ -41,7 +41,7 @@ pub enum RealtimeConfig { /// Helper type for control and motion generation loops. /// /// Used to determine whether to terminate a loop after the control callback has returned. -pub trait ConvertMotion { +pub trait ConvertMotion { /// converts the motion type to a MotionGeneratorCommand and applies rate limiting and filtering fn convert_motion( &self, @@ -140,7 +140,7 @@ impl Finishable for Torques { } } -impl ConvertMotion for Torques { +impl ConvertMotion for Torques { #[allow(unused_variables)] //todo pull convert motion out of the Finishable trait fn convert_motion( @@ -212,10 +212,10 @@ impl Finishable for JointPositions { self } } -impl ConvertMotion for JointPositions { +impl ConvertMotion for JointPositions { fn convert_motion( &self, - robot_state: &PandaState, + robot_state: &RobotState, command: &mut MotionGeneratorCommand, cutoff_frequency: f64, limit_rate: bool, @@ -323,10 +323,10 @@ impl Finishable for JointVelocities { self } } -impl ConvertMotion for JointVelocities { +impl ConvertMotion for JointVelocities { fn convert_motion( &self, - robot_state: &PandaState, + robot_state: &RobotState, command: &mut MotionGeneratorCommand, cutoff_frequency: f64, limit_rate: bool, @@ -475,10 +475,10 @@ impl Finishable for CartesianPose { } } -impl ConvertMotion for CartesianPose { +impl ConvertMotion for CartesianPose { fn convert_motion( &self, - robot_state: &PandaState, + robot_state: &RobotState, command: &mut MotionGeneratorCommand, cutoff_frequency: f64, limit_rate: bool, @@ -665,10 +665,10 @@ impl Finishable for CartesianVelocities { self } } -impl ConvertMotion for CartesianVelocities { +impl ConvertMotion for CartesianVelocities { fn convert_motion( &self, - robot_state: &PandaState, + robot_state: &RobotState, command: &mut MotionGeneratorCommand, cutoff_frequency: f64, limit_rate: bool, diff --git a/src/robot/logger.rs b/src/robot/logger.rs index ed71f3f..2b03b7c 100644 --- a/src/robot/logger.rs +++ b/src/robot/logger.rs @@ -5,7 +5,7 @@ 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; @@ -47,7 +47,7 @@ impl Record { } } -pub(crate) struct Logger { +pub(crate) struct Logger { states: VecDeque, commands: VecDeque, ring_front: usize, @@ -55,7 +55,7 @@ pub(crate) struct Logger { log_size: usize, } -impl Logger { +impl Logger { pub fn new(log_size: usize) -> Self { Logger { states: VecDeque::with_capacity(log_size), diff --git a/src/robot/robot_control.rs b/src/robot/robot_control.rs index e724bfc..3f6a705 100644 --- a/src/robot/robot_control.rs +++ b/src/robot/robot_control.rs @@ -2,12 +2,12 @@ // 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 { - type State: RobotState; + type State: AbstractRobotState; fn start_motion( &mut self, controller_mode: MoveControllerMode, diff --git a/src/robot/robot_impl.rs b/src/robot/robot_impl.rs index 9335554..126e4f1 100644 --- a/src/robot/robot_impl.rs +++ b/src/robot/robot_impl.rs @@ -7,14 +7,14 @@ use crate::robot::control_types::RealtimeConfig; use crate::robot::logger::Logger; use crate::robot::robot_control::RobotControl; -use crate::robot::robot_state::RobotState; +use crate::robot::robot_state::AbstractRobotState; use crate::robot::service_types::{ ConnectResponseWithoutHeader, ConnectStatus, MoveControllerMode, MoveDeviation, - MoveMotionGeneratorMode, MoveRequest, MoveResponse, + MoveMotionGeneratorMode, MoveRequest, }; use crate::robot::types::{ - ControllerCommand, ControllerMode, MotionGeneratorCommand, MotionGeneratorMode, RobotCommand, - RobotStateIntern, + AbstractRobotStateIntern, ControllerCommand, ControllerMode, MotionGeneratorCommand, + MotionGeneratorMode, RobotCommand, }; use crate::RobotModel; use std::fs::remove_file; @@ -173,7 +173,7 @@ impl RobotControl for RobotImplGeneric { // 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); diff --git a/src/robot/robot_state.rs b/src/robot/robot_state.rs index cd31d5c..acd11f7 100644 --- a/src/robot/robot_state.rs +++ b/src/robot/robot_state.rs @@ -9,7 +9,7 @@ use crate::robot::errors::{FrankaErrorKind, FrankaErrors}; use crate::robot::types::{PandaStateIntern, RobotMode}; use nalgebra::{Matrix3, Vector3}; -pub trait RobotState: Clone + Debug { +pub trait AbstractRobotState: Clone + Debug { fn get_time(&self) -> Duration; #[allow(non_snake_case)] fn get_tau_J_d(&self) -> [f64; 7]; @@ -21,7 +21,7 @@ pub trait RobotState: Clone + Debug { /// Describes the robot state. #[derive(Debug, Clone, Default)] #[allow(non_snake_case)] -pub struct PandaState { +pub struct RobotState { /// ![^{O}T_{EE}](https://latex.codecogs.com/png.latex?^{O}T_{EE}) /// /// Measured end effector pose in base frame. @@ -265,9 +265,7 @@ pub struct PandaState { pub time: Duration, } -pub type FR3State = PandaState; - -impl From for PandaState { +impl From for RobotState { #[allow(non_snake_case)] fn from(robot_state: PandaStateIntern) -> Self { let O_T_EE = robot_state.O_T_EE; @@ -332,7 +330,7 @@ impl From for PandaState { let current_errors = FrankaErrors::new(robot_state.errors, FrankaErrorKind::Error); let last_motion_errors = FrankaErrors::new(robot_state.errors, FrankaErrorKind::ReflexReason); - PandaState { + RobotState { O_T_EE, O_T_EE_d, F_T_EE, @@ -451,7 +449,7 @@ fn skew_symmetric_matrix_from_vector(vector: &Vector3) -> Matrix3 { ) } -impl RobotState for PandaState { +impl AbstractRobotState for RobotState { fn get_time(&self) -> Duration { self.time } diff --git a/src/robot/service_types.rs b/src/robot/service_types.rs index 819c0ca..ed244d2 100644 --- a/src/robot/service_types.rs +++ b/src/robot/service_types.rs @@ -348,12 +348,6 @@ define_panda_request_with_header!( ); define_fr3_request_with_header!(MoveRequestWithFR3Header, MoveRequest, FR3CommandEnum::Move); -#[derive(Serialize, Deserialize, Debug)] -pub struct MoveResponse { - pub header: PandaCommandHeader, - pub status: MoveStatusPanda, -} - #[derive(Serialize, Deserialize, Debug, Copy, Clone)] #[repr(packed)] pub struct StopMoveRequestWithPandaHeader { diff --git a/src/robot/types.rs b/src/robot/types.rs index 77dfd56..59e4601 100644 --- a/src/robot/types.rs +++ b/src/robot/types.rs @@ -98,13 +98,13 @@ pub struct PandaStateIntern { pub type FR3StateIntern = PandaStateIntern; -pub trait RobotStateIntern: Copy + Clone + PartialEq { +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 RobotStateIntern for PandaStateIntern { +impl AbstractRobotStateIntern for PandaStateIntern { fn get_message_id(&self) -> u64 { self.message_id } diff --git a/src/utils.rs b/src/utils.rs index 63b8d55..08284b0 100644 --- a/src/utils.rs +++ b/src/utils.rs @@ -3,7 +3,7 @@ //! contains useful type definitions and conversion functions. use crate::robot::control_types::JointPositions; -use crate::robot::robot_state::RobotState; +use crate::robot::robot_state::AbstractRobotState; use crate::Finishable; use nalgebra::{Isometry3, Matrix4, MatrixMN, MatrixN, Rotation3, Vector3, VectorN, U6, U7}; use std::time::Duration; @@ -170,7 +170,7 @@ impl MotionGenerator { /// /// # Return /// Joint positions for use inside a control loop. - pub fn generate_motion( + pub fn generate_motion( &mut self, robot_state: &State, period: &Duration, @@ -193,7 +193,7 @@ impl MotionGenerator { #[cfg(test)] mod test { - use crate::{array_to_isometry, Finishable, MotionGenerator, PandaState}; + use crate::{array_to_isometry, Finishable, MotionGenerator, RobotState}; use nalgebra::Rotation3; use std::time::Duration; @@ -303,7 +303,7 @@ mod test { ], [1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0], ]; - let mut state = PandaState::default(); + let mut state = RobotState::default(); let mut motion_generator = MotionGenerator::new(1.0, &[1.; 7]); let mut joint_pos = motion_generator.generate_motion(&state, &Duration::from_secs_f64(0.0)); slice_compare(&joint_pos.q, &q_des[0], 1e-10); From a83cb4e0fecc75db86d3cc53240a489c0da447b8 Mon Sep 17 00:00:00 2001 From: Marco Boneberger Date: Thu, 6 Apr 2023 13:04:03 +0200 Subject: [PATCH 11/63] abstract over robots in generate_cartesian_pose_motion example --- Cargo.toml | 2 +- examples/generate_cartesian_pose_motion.rs | 34 +++++++++++++++++--- src/lib.rs | 1 + src/network.rs | 2 +- src/robot.rs | 37 +++++++++++++--------- 5 files changed, 55 insertions(+), 21 deletions(-) diff --git a/Cargo.toml b/Cargo.toml index 98048fd..de2c220 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -84,6 +84,6 @@ thiserror = "1.0" libloading = "0.7.0" [dev-dependencies] -clap = { version = "3.1.7", features = ["derive"] } +clap = { version = "3.2.23", features = ["derive"] } mockall = "0.9.1" float_extras = "0.1.6" diff --git a/examples/generate_cartesian_pose_motion.rs b/examples/generate_cartesian_pose_motion.rs index c57c400..85ecf7b 100644 --- a/examples/generate_cartesian_pose_motion.rs +++ b/examples/generate_cartesian_pose_motion.rs @@ -3,7 +3,10 @@ use clap::Parser; use franka::robot::{Robot, FR3}; -use franka::{CartesianPose, RobotState}; +use franka::{ + CartesianPose, CartesianVelocities, ConvertMotion, JointPositions, JointVelocities, Panda, + RobotData, RobotState, +}; use franka::{Finishable, FrankaResult}; use std::f64::consts::PI; use std::time::Duration; @@ -16,11 +19,20 @@ 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 = FR3::new(address.franka_ip.as_str(), None, None)?; +fn generate_motion(mut robot: R) -> FrankaResult<()> +where + CartesianPose: ConvertMotion<<::Data as RobotData>::State>, + CartesianVelocities: ConvertMotion<<::Data as RobotData>::State>, + JointPositions: ConvertMotion<<::Data as RobotData>::State>, + JointVelocities: ConvertMotion<<::Data as RobotData>::State>, + RobotState: From<<::Data as RobotData>::State>, +{ 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..."); @@ -64,3 +76,17 @@ fn main() -> FrankaResult<()> { }; robot.control_cartesian_pose(callback, None, None, None) } + +fn main() -> FrankaResult<()> { + 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) + } + } +} diff --git a/src/lib.rs b/src/lib.rs index 8ceb2b2..1111b4a 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -178,6 +178,7 @@ pub use model::FR3Model; pub use model::Frame; pub use model::PandaModel; pub use model::RobotModel; +pub use network::RobotData; pub use robot::control_types::*; pub use robot::low_pass_filter::DEFAULT_CUTOFF_FREQUENCY; pub use robot::low_pass_filter::MAX_CUTOFF_FREQUENCY; diff --git a/src/network.rs b/src/network.rs index adad706..f001d7a 100644 --- a/src/network.rs +++ b/src/network.rs @@ -76,7 +76,7 @@ pub trait DeviceData { pub trait RobotData: DeviceData { type DeviceData: DeviceData; type Header: RobotHeader; - type State: AbstractRobotState + From; + type State: AbstractRobotState + From + From; type StateIntern: Debug + DeserializeOwned + Serialize + AbstractRobotStateIntern + 'static; type Model: RobotModel; type LoadModelRequestWithHeader: MessageCommand diff --git a/src/robot.rs b/src/robot.rs index cff6e36..caf66cd 100644 --- a/src/robot.rs +++ b/src/robot.rs @@ -27,7 +27,7 @@ use crate::robot::service_types::{ }; use crate::robot::virtual_wall_cuboid::VirtualWallCuboid; use crate::utils::MotionGenerator; -use crate::Finishable; +use crate::{Finishable, RobotState}; mod control_loop; mod control_tools; @@ -51,6 +51,7 @@ where JointVelocities: ConvertMotion<<::Data as RobotData>::State>, JointPositions: ConvertMotion<<::Data as RobotData>::State>, CartesianVelocities: ConvertMotion<<::Data as RobotData>::State>, + RobotState: From<<::Data as RobotData>::State>, { type Data: RobotData; type Rob: RobotImplementation; @@ -79,13 +80,10 @@ where /// 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::Data as RobotData>::State) -> bool>( - &mut self, - mut read_callback: F, - ) -> FrankaResult<()> { + fn read bool>(&mut self, mut read_callback: F) -> FrankaResult<()> { loop { let state = self.get_rob_mut().update(None, None)?; - if !read_callback(&state) { + if !read_callback(&state.into()) { break; } } @@ -390,19 +388,22 @@ where /// /// See [`new`](`Self::new`) to change behavior if realtime priority cannot be set. fn control_joint_positions< - F: FnMut(&<::Data as RobotData>::State, &Duration) -> JointPositions, + 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<()> { + let cb = |state: &<::Data as RobotData>::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(), @@ -432,19 +433,22 @@ where /// /// See [`new`](`Self::new`) to change behavior if realtime priority cannot be set. fn control_joint_velocities< - F: FnMut(&<::Data as RobotData>::State, &Duration) -> JointVelocities, + 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<()> { + let cb = |state: &<::Data as RobotData>::State, duration: &Duration| { + motion_generator_callback(&(state.clone().into()), duration) // todo make this without cloning + }; self.control_motion_intern( - motion_generator_callback, + cb, controller_mode.into(), limit_rate.into(), cutoff_frequency.into(), @@ -475,19 +479,22 @@ where /// /// See [`new`](`Self::new`) to change behavior if realtime priority cannot be set. fn control_cartesian_pose< - F: FnMut(&<::Data as RobotData>::State, &Duration) -> CartesianPose, + 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<()> { + let cb = |state: &<::Data as RobotData>::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(), From bb884bbd3fba766b37c743de6fce8b43505e6ad3 Mon Sep 17 00:00:00 2001 From: Marco Boneberger Date: Thu, 6 Apr 2023 15:41:11 +0200 Subject: [PATCH 12/63] use robot wrapper to hide implementation details in Robot trait --- examples/cartesian_impedance_control.rs | 2 +- examples/communication_test.rs | 2 +- examples/echo_robot_state.rs | 2 +- examples/generate_cartesian_pose_motion.rs | 48 +- .../generate_cartesian_velocity_motion.rs | 2 +- examples/generate_consecutive_motions.rs | 2 +- examples/generate_elbow_motion.rs | 2 +- examples/generate_joint_position_motion.rs | 2 +- examples/generate_joint_velocity_motion.rs | 2 +- examples/mirror_robot.rs | 2 +- examples/print_joint_poses.rs | 2 +- src/robot.rs | 975 +++++++++++------- 12 files changed, 663 insertions(+), 380 deletions(-) diff --git a/examples/cartesian_impedance_control.rs b/examples/cartesian_impedance_control.rs index 633ef8f..cf54765 100644 --- a/examples/cartesian_impedance_control.rs +++ b/examples/cartesian_impedance_control.rs @@ -3,7 +3,7 @@ use clap::Parser; use franka::robot::robot_state::RobotState; -use franka::robot::{Robot, FR3}; +use franka::robot::{Robot, RobotWrapper, FR3}; use franka::FrankaResult; use franka::Torques; use franka::{array_to_isometry, Matrix6x7, Vector7}; diff --git a/examples/communication_test.rs b/examples/communication_test.rs index bb5ae1e..f39e943 100644 --- a/examples/communication_test.rs +++ b/examples/communication_test.rs @@ -1,7 +1,7 @@ // Copyright (c) 2021 Marco Boneberger // Licensed under the EUPL-1.2-or-later use clap::Parser; -use franka::robot::{Robot, FR3}; +use franka::robot::{Robot, RobotWrapper, FR3}; use franka::Torques; use franka::{Finishable, FrankaResult, RobotState}; use std::f64::consts::PI; diff --git a/examples/echo_robot_state.rs b/examples/echo_robot_state.rs index 0122ae7..19867fe 100644 --- a/examples/echo_robot_state.rs +++ b/examples/echo_robot_state.rs @@ -2,7 +2,7 @@ // Licensed under the EUPL-1.2-or-later use clap::Parser; -use franka::robot::{Robot, FR3}; +use franka::robot::{Robot, RobotWrapper, FR3}; use franka::{FrankaResult, RobotState}; /// An example showing how to continuously read the robot state. diff --git a/examples/generate_cartesian_pose_motion.rs b/examples/generate_cartesian_pose_motion.rs index 85ecf7b..9d48f69 100644 --- a/examples/generate_cartesian_pose_motion.rs +++ b/examples/generate_cartesian_pose_motion.rs @@ -2,7 +2,7 @@ // Licensed under the EUPL-1.2-or-later use clap::Parser; -use franka::robot::{Robot, FR3}; +use franka::robot::{Robot, RobotWrapper, FR3}; use franka::{ CartesianPose, CartesianVelocities, ConvertMotion, JointPositions, JointVelocities, Panda, RobotData, RobotState, @@ -77,16 +77,56 @@ where robot.control_cartesian_pose(callback, None, None, None) } +// fn get_robot(t:bool +// ) -> Box { +// match t { +// true => { Box::new(Panda::new("localhost", None, None).unwrap())} +// false => {Box::new(FR3::new("localhost", None, None).unwrap())} +// } +// } + +fn dyn_generate_motion(mut robot: R) -> FrankaResult<()> +// where + // CartesianPose: ConvertMotion<<::Data as RobotData>::State>, + // CartesianVelocities: ConvertMotion<<::Data as RobotData>::State>, + // JointPositions: ConvertMotion<<::Data as RobotData>::State>, + // JointVelocities: ConvertMotion<<::Data as RobotData>::State>, + // RobotState: From<<::Data as RobotData>::State>, +{ + 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 + // should not be done in a control loop. + println!("{:?}", robot_state); + count += 1; + count <= 100 + }); + let q_goal = [0., -PI / 4., 0., -3. * PI / 4., 0., PI / 2., PI / 4.]; + let res = robot.joint_motion(0.5, &q_goal); + println!("Finished moving to initial joint configuration."); + res +} + fn main() -> FrankaResult<()> { let address = CommandLineArguments::parse(); match address.panda { true => { let robot = Panda::new(address.franka_ip.as_str(), None, None)?; - generate_motion(robot) + dyn_generate_motion(robot) } false => { - let robot = FR3::new(address.franka_ip.as_str(), None, None)?; - generate_motion(robot) + let mut robot = FR3::new(address.franka_ip.as_str(), None, None)?; + let mut count = 0; + println!("{:?}", robot.read_once()?); + Ok(()) + // robot.read(|robot_state: &RobotState| { + // // Printing to standard output adds a delay. This is acceptable for a read loop such as this, but + // // should not be done in a control loop. + // println!("{:?}", robot_state); + // count += 1; + // count <= 100 + // }); + // dyn_generate_motion(robot) } } } diff --git a/examples/generate_cartesian_velocity_motion.rs b/examples/generate_cartesian_velocity_motion.rs index 6af6abc..21c3b6f 100644 --- a/examples/generate_cartesian_velocity_motion.rs +++ b/examples/generate_cartesian_velocity_motion.rs @@ -3,7 +3,7 @@ use clap::Parser; use franka::robot::robot_state::RobotState; -use franka::robot::{Robot, FR3}; +use franka::robot::{Robot, RobotWrapper, FR3}; use franka::{CartesianVelocities, Finishable, FrankaResult}; use std::f64::consts::PI; use std::time::Duration; diff --git a/examples/generate_consecutive_motions.rs b/examples/generate_consecutive_motions.rs index 4b98277..bc3a461 100644 --- a/examples/generate_consecutive_motions.rs +++ b/examples/generate_consecutive_motions.rs @@ -3,7 +3,7 @@ use clap::Parser; use franka::exception::FrankaException; -use franka::robot::{Robot, FR3}; +use franka::robot::{Robot, RobotWrapper, FR3}; use franka::{Finishable, FrankaResult, JointVelocities, RobotState}; use std::f64::consts::PI; use std::time::Duration; diff --git a/examples/generate_elbow_motion.rs b/examples/generate_elbow_motion.rs index 9ef9edc..38b8055 100644 --- a/examples/generate_elbow_motion.rs +++ b/examples/generate_elbow_motion.rs @@ -2,7 +2,7 @@ // Licensed under the EUPL-1.2-or-later use clap::Parser; -use franka::robot::{Robot, FR3}; +use franka::robot::{Robot, RobotWrapper, FR3}; use franka::{CartesianPose, Finishable, FrankaResult, RobotState}; use std::f64::consts::PI; use std::time::Duration; diff --git a/examples/generate_joint_position_motion.rs b/examples/generate_joint_position_motion.rs index dcca9ae..1e63f3e 100644 --- a/examples/generate_joint_position_motion.rs +++ b/examples/generate_joint_position_motion.rs @@ -2,7 +2,7 @@ // Licensed under the EUPL-1.2-or-later use clap::Parser; -use franka::robot::{Robot, FR3}; +use franka::robot::{Robot, RobotWrapper, FR3}; use franka::{Finishable, FrankaResult, JointPositions, RobotState}; use std::f64::consts::PI; use std::time::Duration; diff --git a/examples/generate_joint_velocity_motion.rs b/examples/generate_joint_velocity_motion.rs index 02738ab..4a4d016 100644 --- a/examples/generate_joint_velocity_motion.rs +++ b/examples/generate_joint_velocity_motion.rs @@ -2,7 +2,7 @@ // Licensed under the EUPL-1.2-or-later use clap::Parser; -use franka::robot::{Robot, FR3}; +use franka::robot::{Robot, RobotWrapper, FR3}; use franka::JointVelocities; use franka::RobotState; use franka::{Finishable, FrankaResult}; diff --git a/examples/mirror_robot.rs b/examples/mirror_robot.rs index c814b79..bbbbd80 100644 --- a/examples/mirror_robot.rs +++ b/examples/mirror_robot.rs @@ -5,7 +5,7 @@ use core::f64::consts::PI; use franka::exception::FrankaResult; use franka::model::Frame; use franka::robot::control_types::Torques; -use franka::robot::{Robot, FR3}; +use franka::robot::{Robot, RobotWrapper, FR3}; use franka::utils::{array_to_isometry, Matrix6x7, Vector7}; use franka::RobotState; use franka::{Matrix7, RobotModel}; diff --git a/examples/print_joint_poses.rs b/examples/print_joint_poses.rs index 986ca0b..9e06ddb 100644 --- a/examples/print_joint_poses.rs +++ b/examples/print_joint_poses.rs @@ -1,7 +1,7 @@ // Copyright (c) 2021 Marco Boneberger // Licensed under the EUPL-1.2-or-later use clap::Parser; -use franka::robot::{Robot, FR3}; +use franka::robot::{Robot, RobotWrapper, FR3}; use franka::{Frame, FrankaResult, RealtimeConfig, RobotModel}; use nalgebra::Matrix4; diff --git a/src/robot.rs b/src/robot.rs index caf66cd..51d313d 100644 --- a/src/robot.rs +++ b/src/robot.rs @@ -45,19 +45,7 @@ pub(crate) mod service_types; pub(crate) mod types; pub mod virtual_wall_cuboid; -pub trait Robot -where - CartesianPose: ConvertMotion<<::Data as RobotData>::State>, - JointVelocities: ConvertMotion<<::Data as RobotData>::State>, - JointPositions: ConvertMotion<<::Data as RobotData>::State>, - CartesianVelocities: ConvertMotion<<::Data as RobotData>::State>, - RobotState: From<<::Data as RobotData>::State>, -{ - type Data: RobotData; - 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; +pub trait RobotWrapper { /// Starts a loop for reading the current robot state. /// /// Cannot be executed while a control or motion generator loop is running. @@ -80,15 +68,53 @@ where /// 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, mut read_callback: F) -> FrankaResult<()> { - loop { - let state = self.get_rob_mut().update(None, None)?; - if !read_callback(&state.into()) { - break; - } - } - Ok(()) - } + fn read bool>(&mut self, read_callback: F) -> 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` 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. + 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<()>; + + /// 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<()>; + /// Waits for a robot state update and returns it. /// /// # Return @@ -97,9 +123,8 @@ where /// * [`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<<::Data as RobotData>::State> { - self.get_rob_mut().read_once() - } + fn read_once(&mut self) -> FrankaResult; + /// Changes the collision behavior. /// /// Set separate torque and force boundaries for acceleration/deceleration and constant velocity @@ -142,24 +167,13 @@ where upper_force_thresholds_acceleration: [f64; 6], lower_force_thresholds_nominal: [f64; 6], upper_force_thresholds_nominal: [f64; 6], - ) -> FrankaResult<()> { - let command = Self::Data::create_set_collision_behavior_request( - &mut self.get_net().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 = self.get_net().tcp_send_request(command); - let status = self.get_net().tcp_blocking_receive_status(command_id); - Self::Data::handle_getter_setter_status(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. /// @@ -170,15 +184,8 @@ where /// * [`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_joint_impedance(&mut self, K_theta: [f64; 7]) -> FrankaResult<()> { - let command = Self::Data::create_set_joint_impedance_request( - &mut self.get_net().command_id, - SetJointImpedanceRequest::new(K_theta), - ); - let command_id: u32 = self.get_net().tcp_send_request(command); - let status = self.get_net().tcp_blocking_receive_status(command_id); - Self::Data::handle_getter_setter_status(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. /// /// User-provided torques are not affected by this setting. @@ -190,15 +197,7 @@ where /// * [`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_cartesian_impedance(&mut self, K_x: [f64; 6]) -> FrankaResult<()> { - let command = Self::Data::create_set_cartesian_impedance_request( - &mut self.get_net().command_id, - SetCartesianImpedanceRequest::new(K_x), - ); - let command_id: u32 = self.get_net().tcp_send_request(command); - let status = self.get_net().tcp_blocking_receive_status(command_id); - Self::Data::handle_getter_setter_status(status) - } + fn set_cartesian_impedance(&mut self, K_x: [f64; 6]) -> FrankaResult<()>; /// Sets dynamic parameters of a payload. /// @@ -220,15 +219,7 @@ where load_mass: f64, F_x_Cload: [f64; 3], load_inertia: [f64; 9], - ) -> FrankaResult<()> { - let command = Self::Data::create_set_load_request( - &mut self.get_net().command_id, - SetLoadRequest::new(load_mass, F_x_Cload, load_inertia), - ); - let command_id: u32 = self.get_net().tcp_send_request(command); - let status = self.get_net().tcp_blocking_receive_status(command_id); - Self::Data::handle_getter_setter_status(status) - } + ) -> FrankaResult<()>; /// Locks or unlocks guiding mode movement in (x, y, z, roll, pitch, yaw). /// @@ -241,15 +232,7 @@ where /// # 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_guiding_mode(&mut self, guiding_mode: [bool; 6], elbow: bool) -> FrankaResult<()> { - let command = Self::Data::create_set_guiding_mode_request( - &mut self.get_net().command_id, - SetGuidingModeRequest::new(guiding_mode, elbow), - ); - let command_id: u32 = self.get_net().tcp_send_request(command); - let status = self.get_net().tcp_blocking_receive_status(command_id); - Self::Data::handle_getter_setter_status(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. /// @@ -262,15 +245,7 @@ where /// /// See [Stiffness frame K](#stiffness-frame-k) for an explanation of the stiffness frame. #[allow(non_snake_case)] - fn set_K(&mut self, EE_T_K: [f64; 16]) -> FrankaResult<()> { - let command = Self::Data::create_set_ee_to_k_request( - &mut self.get_net().command_id, - SetEeToKRequest::new(EE_T_K), - ); - let command_id: u32 = self.get_net().tcp_send_request(command); - let status = self.get_net().tcp_blocking_receive_status(command_id); - Self::Data::handle_getter_setter_status(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. /// @@ -287,15 +262,7 @@ where /// * [`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)] - fn set_EE(&mut self, NE_T_EE: [f64; 16]) -> FrankaResult<()> { - let command = Self::Data::create_set_ne_to_ee_request( - &mut self.get_net().command_id, - SetNeToEeRequest::new(NE_T_EE), - ); - let command_id: u32 = self.get_net().tcp_send_request(command); - let status = self.get_net().tcp_blocking_receive_status(command_id); - Self::Data::handle_getter_setter_status(status) - } + fn set_EE(&mut self, NE_T_EE: [f64; 16]) -> FrankaResult<()>; ///Runs automatic error recovery on the robot. /// @@ -303,13 +270,7 @@ where /// # 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 automatic_error_recovery(&mut self) -> FrankaResult<()> { - let command = - Self::Data::create_automatic_error_recovery_request(&mut self.get_net().command_id); - let command_id: u32 = self.get_net().tcp_send_request(command); - let status = self.get_net().tcp_blocking_receive_status(command_id); - Self::Data::handle_automatic_error_recovery_status(status) - } + fn automatic_error_recovery(&mut self) -> FrankaResult<()>; /// Stops all currently running motions. /// @@ -318,52 +279,9 @@ where /// # 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 stop(&mut self) -> FrankaResult<()> { - let command = Self::Data::create_stop_request(&mut self.get_net().command_id); - let command_id: u32 = self.get_net().tcp_send_request(command); - let status: StopMoveStatusPanda = self.get_net().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 stop(&mut self) -> FrankaResult<()>; - fn control_motion_intern< - F: FnMut(&<::Data as RobotData>::State, &Duration) -> U, - U: ConvertMotion<<::Data as RobotData>::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(true); - 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() - } - /// Starts a control loop for a joint position motion generator with a given controller mode. + /// 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. @@ -378,38 +296,28 @@ where /// 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. + /// * if joint velocity commands are NaN or infinity. /// /// See [`new`](`Self::new`) to change behavior if realtime priority cannot be set. - fn control_joint_positions< - F: FnMut(&RobotState, &Duration) -> JointPositions, + fn control_joint_velocities< + F: FnMut(&RobotState, &Duration) -> JointVelocities, CM: Into>, L: Into>, CF: Into>, >( &mut self, - mut motion_generator_callback: F, + motion_generator_callback: F, controller_mode: CM, limit_rate: L, cutoff_frequency: CF, - ) -> FrankaResult<()> { - let cb = |state: &<::Data as RobotData>::State, duration: &Duration| { - motion_generator_callback(&(state.clone().into()), duration) - }; - self.control_motion_intern( - 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. + ) -> 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. @@ -424,37 +332,29 @@ where /// 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. + /// * if Cartesian pose command elements are NaN or infinity. /// /// See [`new`](`Self::new`) to change behavior if realtime priority cannot be set. - fn control_joint_velocities< - F: FnMut(&RobotState, &Duration) -> JointVelocities, + fn control_cartesian_pose< + F: FnMut(&RobotState, &Duration) -> CartesianPose, CM: Into>, L: Into>, CF: Into>, >( &mut self, - mut motion_generator_callback: F, + motion_generator_callback: F, controller_mode: CM, limit_rate: L, cutoff_frequency: CF, - ) -> FrankaResult<()> { - let cb = |state: &<::Data as RobotData>::State, duration: &Duration| { - motion_generator_callback(&(state.clone().into()), duration) // todo make this without cloning - }; - self.control_motion_intern( - 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. + ) -> 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. @@ -475,151 +375,58 @@ where /// * [`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. + /// * if Cartesian velocity command elements are NaN or infinity. /// /// See [`new`](`Self::new`) to change behavior if realtime priority cannot be set. - fn control_cartesian_pose< - F: FnMut(&RobotState, &Duration) -> CartesianPose, + fn control_cartesian_velocities< + F: FnMut(&RobotState, &Duration) -> CartesianVelocities, CM: Into>, L: Into>, CF: Into>, >( &mut self, - mut motion_generator_callback: F, + motion_generator_callback: F, controller_mode: CM, limit_rate: L, cutoff_frequency: CF, - ) -> FrankaResult<()> { - let cb = |state: &<::Data as RobotData>::State, duration: &Duration| { - motion_generator_callback(&(state.clone().into()), duration) - }; - self.control_motion_intern( - cb, - controller_mode.into(), - limit_rate.into(), - cutoff_frequency.into(), - ) - } + ) -> FrankaResult<()>; - /// Starts a control loop for a Cartesian velocity motion generator with a given controller mode. + /// Starts a control loop for sending joint-level torque commands. /// - /// Sets realtime priority for the current thread. + /// Sets real-time 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. + /// * `control_callback` - Callback function providing joint-level torque commands. /// 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. + /// * `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 + /// * `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. + /// * [`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 Cartesian velocity command elements are NaN or infinity. + /// * if joint-level torque commands are NaN or infinity. /// - /// See [`new`](`Self::new`) to change behavior if realtime priority cannot be set. - fn control_cartesian_velocities< - F: FnMut(&<::Data as RobotData>::State, &Duration) -> CartesianVelocities, - CM: Into>, + /// See [`new`](`Self::new`) to change behavior if real-time priority cannot be set. + fn control_torques< + T: FnMut(&RobotState, &Duration) -> Torques, L: Into>, CF: Into>, >( &mut self, - motion_generator_callback: F, - controller_mode: CM, + control_callback: T, limit_rate: L, cutoff_frequency: CF, - ) -> FrankaResult<()> { - self.control_motion_intern( - motion_generator_callback, - controller_mode.into(), - limit_rate.into(), - cutoff_frequency.into(), - ) - } - - fn control_torques_intern< - F: FnMut(&<::Data as RobotData>::State, &Duration) -> U, - U: ConvertMotion<<::Data as RobotData>::State> - + Debug - + MotionGeneratorTrait - + Finishable, - >( - &mut self, - motion_generator_callback: F, - control_callback: &mut dyn FnMut( - &<::Data as RobotData>::State, - &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( - self.get_rob_mut(), - control_callback, - motion_generator_callback, - limit_rate, - cutoff_frequency, - )?; - control_loop.run() - } - - /// 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. - fn control_torques< - T: FnMut(&<::Data as RobotData>::State, &Duration) -> Torques, - L: Into>, - CF: Into>, - >( - &mut self, - mut control_callback: T, - limit_rate: L, - cutoff_frequency: CF, - ) -> FrankaResult<()> { - let motion_generator_callback = - |_state: &<::Data as RobotData>::State, _time_step: &Duration| { - JointVelocities::new([0.; 7]) - }; - self.control_torques_intern( - &motion_generator_callback, - &mut control_callback, - limit_rate.into(), - cutoff_frequency.into(), - ) - } + ) -> FrankaResult<()>; /// Starts a control loop for sending joint-level torque commands and joint velocities. /// @@ -646,24 +453,18 @@ where /// /// See [`new`](`Self::new`) to change behavior if realtime priority cannot be set. fn control_torques_and_joint_velocities< - F: FnMut(&<::Data as RobotData>::State, &Duration) -> JointVelocities, - T: FnMut(&<::Data as RobotData>::State, &Duration) -> Torques, + F: FnMut(&RobotState, &Duration) -> JointVelocities, + T: FnMut(&RobotState, &Duration) -> Torques, L: Into>, CF: Into>, >( &mut self, - mut control_callback: T, + control_callback: T, motion_generator_callback: F, limit_rate: L, cutoff_frequency: CF, - ) -> FrankaResult<()> { - self.control_torques_intern( - motion_generator_callback, - &mut control_callback, - limit_rate.into(), - cutoff_frequency.into(), - ) - } + ) -> FrankaResult<()>; + /// Starts a control loop for sending joint-level torque commands and joint positions. /// /// Sets realtime priority for the current thread. @@ -689,24 +490,17 @@ where /// /// See [`new`](`Self::new`) to change behavior if realtime priority cannot be set. fn control_torques_and_joint_positions< - F: FnMut(&<::Data as RobotData>::State, &Duration) -> JointPositions, - T: FnMut(&<::Data as RobotData>::State, &Duration) -> Torques, + F: FnMut(&RobotState, &Duration) -> JointPositions, + T: FnMut(&RobotState, &Duration) -> Torques, L: Into>, CF: Into>, >( &mut self, - mut control_callback: T, + control_callback: T, motion_generator_callback: F, limit_rate: L, cutoff_frequency: CF, - ) -> FrankaResult<()> { - self.control_torques_intern( - motion_generator_callback, - &mut control_callback, - limit_rate.into(), - cutoff_frequency.into(), - ) - } + ) -> FrankaResult<()>; /// Starts a control loop for sending joint-level torque commands and Cartesian poses. /// @@ -733,24 +527,17 @@ where /// /// See [`new`](`Self::new`) to change behavior if realtime priority cannot be set. fn control_torques_and_cartesian_pose< - F: FnMut(&<::Data as RobotData>::State, &Duration) -> CartesianPose, - T: FnMut(&<::Data as RobotData>::State, &Duration) -> Torques, + F: FnMut(&RobotState, &Duration) -> CartesianPose, + T: FnMut(&RobotState, &Duration) -> Torques, L: Into>, CF: Into>, >( &mut self, - mut control_callback: T, + control_callback: T, motion_generator_callback: F, limit_rate: L, cutoff_frequency: CF, - ) -> FrankaResult<()> { - self.control_torques_intern( - motion_generator_callback, - &mut control_callback, - limit_rate.into(), - cutoff_frequency.into(), - ) - } + ) -> FrankaResult<()>; /// Starts a control loop for sending joint-level torque commands and Cartesian velocities. /// @@ -777,55 +564,269 @@ where /// /// See [`new`](`Self::new`) to change behavior if realtime priority cannot be set. fn control_torques_and_cartesian_velocities< - F: FnMut(&<::Data as RobotData>::State, &Duration) -> CartesianVelocities, - T: FnMut(&<::Data as RobotData>::State, &Duration) -> Torques, + F: FnMut(&RobotState, &Duration) -> CartesianVelocities, + T: FnMut(&RobotState, &Duration) -> Torques, L: Into>, CF: Into>, >( &mut self, - mut control_callback: T, + control_callback: T, motion_generator_callback: F, limit_rate: L, cutoff_frequency: CF, + ) -> FrankaResult<()>; +} + +impl RobotWrapper for R +where + RobotState: From<<::Data as RobotData>::State>, + CartesianPose: control_types::ConvertMotion<<::Data as RobotData>::State>, + JointVelocities: control_types::ConvertMotion<<::Data as RobotData>::State>, + JointPositions: control_types::ConvertMotion<<::Data as RobotData>::State>, + CartesianVelocities: control_types::ConvertMotion<<::Data as RobotData>::State>, +{ + 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(()) + } + + fn control_joint_positions< + F: FnMut(&RobotState, &Duration) -> JointPositions, + CM: Into>, + L: Into>, + CF: Into>, + >( + &mut self, + mut motion_generator_callback: F, + controller_mode: CM, + limit_rate: L, + cutoff_frequency: CF, + ) -> FrankaResult<()> { + let cb = |state: &<::Data as RobotData>::State, duration: &Duration| { + motion_generator_callback(&(state.clone().into()), duration) + }; + ::control_motion_intern( + self, + cb, + controller_mode.into(), + limit_rate.into(), + cutoff_frequency.into(), + ) + } + + fn control_joint_velocities< + F: FnMut(&RobotState, &Duration) -> JointVelocities, + CM: Into>, + L: Into>, + CF: Into>, + >( + &mut self, + mut motion_generator_callback: F, + controller_mode: CM, + limit_rate: L, + cutoff_frequency: CF, + ) -> FrankaResult<()> { + let cb = |state: &<::Data as RobotData>::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(), + ) + } + + fn control_cartesian_pose< + F: FnMut(&RobotState, &Duration) -> CartesianPose, + CM: Into>, + L: Into>, + CF: Into>, + >( + &mut self, + mut motion_generator_callback: F, + controller_mode: CM, + limit_rate: L, + cutoff_frequency: CF, ) -> FrankaResult<()> { + let cb = |state: &<::Data as RobotData>::State, duration: &Duration| { + motion_generator_callback(&(state.clone().into()), duration) + }; + ::control_motion_intern( + self, + cb, + controller_mode.into(), + limit_rate.into(), + cutoff_frequency.into(), + ) + } + + fn control_cartesian_velocities< + F: FnMut(&RobotState, &Duration) -> CartesianVelocities, + CM: Into>, + L: Into>, + CF: Into>, + >( + &mut self, + mut motion_generator_callback: F, + controller_mode: CM, + limit_rate: L, + cutoff_frequency: CF, + ) -> FrankaResult<()> { + let cb = |state: &<::Data as RobotData>::State, duration: &Duration| { + motion_generator_callback(&(state.clone().into()), duration) + }; + self.control_motion_intern( + cb, + controller_mode.into(), + limit_rate.into(), + cutoff_frequency.into(), + ) + } + + fn control_torques< + T: FnMut(&RobotState, &Duration) -> Torques, + L: Into>, + CF: Into>, + >( + &mut self, + mut control_callback: T, + limit_rate: L, + cutoff_frequency: CF, + ) -> FrankaResult<()> { + let motion_generator_callback = + |_state: &<::Data as RobotData>::State, _time_step: &Duration| { + JointVelocities::new([0.; 7]) + }; + let mut cb = |state: &<::Data as RobotData>::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(), ) } - /// 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. - fn load_model(&mut self, persistent: bool) -> FrankaResult<::Model> { - self.get_rob_mut().load_model(persistent) + 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, + mut motion_generator_callback: F, + limit_rate: L, + cutoff_frequency: CF, + ) -> FrankaResult<()> { + let motion_generator_cb = |state: &<::Data as RobotData>::State, + duration: &Duration| { + motion_generator_callback(&(state.clone().into()), duration) + }; + let mut control_cb = |state: &<::Data as RobotData>::State, + duration: &Duration| { + control_callback(&(state.clone().into()), duration) + }; + self.control_torques_intern( + motion_generator_cb, + &mut control_cb, + limit_rate.into(), + cutoff_frequency.into(), + ) } - /// 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<()> { - self.set_collision_behavior( - [20.; 7], [20.; 7], [10.; 7], [10.; 7], [20.; 6], [20.; 6], [10.; 6], [10.; 6], - )?; - self.set_joint_impedance([3000., 3000., 3000., 2500., 2500., 2000., 2000.])?; - self.set_cartesian_impedance([3000., 3000., 3000., 300., 300., 300.])?; - Ok(()) + fn control_torques_and_joint_positions< + F: FnMut(&RobotState, &Duration) -> JointPositions, + 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_cb = |state: &<::Data as RobotData>::State, + duration: &Duration| { + motion_generator_callback(&(state.clone().into()), duration) + }; + let mut control_cb = |state: &<::Data as RobotData>::State, + duration: &Duration| { + control_callback(&(state.clone().into()), duration) + }; + self.control_torques_intern( + motion_generator_cb, + &mut control_cb, + limit_rate.into(), + cutoff_frequency.into(), + ) } - /// 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 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, + mut motion_generator_callback: F, + limit_rate: L, + cutoff_frequency: CF, + ) -> FrankaResult<()> { + let motion_generator_cb = |state: &<::Data as RobotData>::State, + duration: &Duration| { + motion_generator_callback(&(state.clone().into()), duration) + }; + let mut control_cb = |state: &<::Data as RobotData>::State, + duration: &Duration| { + control_callback(&(state.clone().into()), duration) + }; + self.control_torques_intern( + motion_generator_cb, + &mut control_cb, + limit_rate.into(), + cutoff_frequency.into(), + ) + } + + 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_cb = |state: &<::Data as RobotData>::State, + duration: &Duration| { + motion_generator_callback(&(state.clone().into()), duration) + }; + let mut control_cb = |state: &<::Data as RobotData>::State, + duration: &Duration| { + control_callback(&(state.clone().into()), duration) + }; + self.control_torques_intern( + motion_generator_cb, + &mut control_cb, + limit_rate.into(), + cutoff_frequency.into(), + ) + } + 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( @@ -835,6 +836,248 @@ where Some(MAX_CUTOFF_FREQUENCY), ) } + + fn read_once(&mut self) -> FrankaResult { + match ::get_rob_mut(self).read_once() { + Ok(state) => Ok(state.into()), + Err(e) => Err(e), + } + } + + #[allow(clippy::too_many_arguments)] + fn set_collision_behavior( + &mut self, + 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 command = ::Data::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); + ::Data::handle_getter_setter_status(status) + } + + 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], + )?; + self.set_joint_impedance([3000., 3000., 3000., 2500., 2500., 2000., 2000.])?; + self.set_cartesian_impedance([3000., 3000., 3000., 300., 300., 300.])?; + Ok(()) + } + + #[allow(non_snake_case)] + fn set_joint_impedance(&mut self, K_theta: [f64; 7]) -> FrankaResult<()> { + let command = ::Data::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); + ::Data::handle_getter_setter_status(status) + } + + #[allow(non_snake_case)] + fn set_cartesian_impedance(&mut self, K_x: [f64; 6]) -> FrankaResult<()> { + let command = ::Data::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); + ::Data::handle_getter_setter_status(status) + } + + #[allow(non_snake_case)] + fn set_load( + &mut self, + load_mass: f64, + F_x_Cload: [f64; 3], + load_inertia: [f64; 9], + ) -> FrankaResult<()> { + let command = ::Data::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); + ::Data::handle_getter_setter_status(status) + } + + fn set_guiding_mode(&mut self, guiding_mode: [bool; 6], elbow: bool) -> FrankaResult<()> { + let command = ::Data::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); + ::Data::handle_getter_setter_status(status) + } + + #[allow(non_snake_case)] + fn set_K(&mut self, EE_T_K: [f64; 16]) -> FrankaResult<()> { + let command = ::Data::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); + ::Data::handle_getter_setter_status(status) + } + + #[allow(non_snake_case)] + fn set_EE(&mut self, NE_T_EE: [f64; 16]) -> FrankaResult<()> { + let command = ::Data::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); + ::Data::handle_getter_setter_status(status) + } + + fn automatic_error_recovery(&mut self) -> FrankaResult<()> { + let command = ::Data::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); + ::Data::handle_automatic_error_recovery_status(status) + } + + fn stop(&mut self) -> FrankaResult<()> { + let command = ::Data::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!", + )), + } + } +} + +// impl RobotWrapper for FR3 { +// fn read bool>(&mut self, mut read_callback: F) -> FrankaResult<()> { +// loop { +// let state = self.get_rob_mut().update(None, None)?; +// if !read_callback(&state.into()) { +// break; +// } +// } +// Ok(()) +// } +// } + +pub trait Robot +where + CartesianPose: ConvertMotion<<::Data as RobotData>::State>, + JointVelocities: ConvertMotion<<::Data as RobotData>::State>, + JointPositions: ConvertMotion<<::Data as RobotData>::State>, + CartesianVelocities: ConvertMotion<<::Data as RobotData>::State>, + RobotState: From<<::Data as RobotData>::State>, +{ + type Data: RobotData; + 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(&<::Data as RobotData>::State, &Duration) -> U, + U: ConvertMotion<<::Data as RobotData>::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(true); + 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(&<::Data as RobotData>::State, &Duration) -> U, + U: ConvertMotion<<::Data as RobotData>::State> + + Debug + + MotionGeneratorTrait + + Finishable, + >( + &mut self, + motion_generator_callback: F, + control_callback: &mut dyn FnMut( + &<::Data as RobotData>::State, + &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( + self.get_rob_mut(), + control_callback, + motion_generator_callback, + limit_rate, + cutoff_frequency, + )?; + control_loop.run() + } + + /// 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. + fn load_model(&mut self, persistent: bool) -> FrankaResult<::Model> { + self.get_rob_mut().load_model(persistent) + } + /// Returns the software version reported by the connected server. /// /// # Return @@ -1094,7 +1337,7 @@ mod tests { SetterResponseFR3, COMMAND_PORT, FR3_VERSION, }; use crate::robot::types::PandaStateIntern; - use crate::robot::{Robot, FR3}; + use crate::robot::{Robot, RobotWrapper, FR3}; use crate::{Finishable, FrankaResult, JointPositions, Panda, RealtimeConfig, RobotState}; use bincode::{deserialize, serialize, serialized_size}; use std::iter::FromIterator; From 0e5b830836fd7c7776debc130030b7f434e521cd Mon Sep 17 00:00:00 2001 From: Marco Boneberger Date: Thu, 6 Apr 2023 17:27:03 +0200 Subject: [PATCH 13/63] make RobotModel trait usable and increase buffer size to download model library --- examples/cartesian_impedance_control.rs | 28 ++++++++++--- examples/generate_cartesian_pose_motion.rs | 49 +++------------------- src/model.rs | 41 +++++++++--------- src/model/library_downloader.rs | 4 +- src/network.rs | 36 +++++++++------- src/robot/robot_impl.rs | 2 +- src/robot/service_types.rs | 6 --- 7 files changed, 69 insertions(+), 97 deletions(-) diff --git a/examples/cartesian_impedance_control.rs b/examples/cartesian_impedance_control.rs index cf54765..517fb34 100644 --- a/examples/cartesian_impedance_control.rs +++ b/examples/cartesian_impedance_control.rs @@ -3,7 +3,7 @@ use clap::Parser; use franka::robot::robot_state::RobotState; -use franka::robot::{Robot, RobotWrapper, FR3}; +use franka::robot::{Panda, Robot, RobotWrapper, FR3}; use franka::FrankaResult; use franka::Torques; use franka::{array_to_isometry, Matrix6x7, Vector7}; @@ -21,10 +21,13 @@ 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(); +fn generate_motion(mut robot: R, model: M) -> FrankaResult<()> { let translational_stiffness = 150.; let rotational_stiffness = 10.; @@ -43,9 +46,6 @@ fn main() -> FrankaResult<()> { bottom_right_corner .copy_from(&(2. * f64::sqrt(rotational_stiffness) * Matrix3::identity())); } - let mut robot = FR3::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( @@ -112,3 +112,19 @@ fn main() -> FrankaResult<()> { } } } + +fn main() -> FrankaResult<()> { + let address = CommandLineArguments::parse(); + match address.panda { + true => { + let mut robot = Panda::new(address.franka_ip.as_str(), None, None)?; + let model = robot.load_model(false).unwrap(); + generate_motion(robot, model) + } + false => { + let mut robot = FR3::new(address.franka_ip.as_str(), None, None)?; + let model = robot.load_model(false).unwrap(); + generate_motion(robot, model) + } + } +} diff --git a/examples/generate_cartesian_pose_motion.rs b/examples/generate_cartesian_pose_motion.rs index 9d48f69..fe36379 100644 --- a/examples/generate_cartesian_pose_motion.rs +++ b/examples/generate_cartesian_pose_motion.rs @@ -2,10 +2,11 @@ // Licensed under the EUPL-1.2-or-later use clap::Parser; +use franka::model::RobotModel; use franka::robot::{Robot, RobotWrapper, FR3}; use franka::{ - CartesianPose, CartesianVelocities, ConvertMotion, JointPositions, JointVelocities, Panda, - RobotData, RobotState, + CartesianPose, CartesianVelocities, ConvertMotion, Frame, JointPositions, JointVelocities, + Panda, RobotData, RobotState, }; use franka::{Finishable, FrankaResult}; use std::f64::consts::PI; @@ -77,56 +78,16 @@ where robot.control_cartesian_pose(callback, None, None, None) } -// fn get_robot(t:bool -// ) -> Box { -// match t { -// true => { Box::new(Panda::new("localhost", None, None).unwrap())} -// false => {Box::new(FR3::new("localhost", None, None).unwrap())} -// } -// } - -fn dyn_generate_motion(mut robot: R) -> FrankaResult<()> -// where - // CartesianPose: ConvertMotion<<::Data as RobotData>::State>, - // CartesianVelocities: ConvertMotion<<::Data as RobotData>::State>, - // JointPositions: ConvertMotion<<::Data as RobotData>::State>, - // JointVelocities: ConvertMotion<<::Data as RobotData>::State>, - // RobotState: From<<::Data as RobotData>::State>, -{ - 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 - // should not be done in a control loop. - println!("{:?}", robot_state); - count += 1; - count <= 100 - }); - let q_goal = [0., -PI / 4., 0., -3. * PI / 4., 0., PI / 2., PI / 4.]; - let res = robot.joint_motion(0.5, &q_goal); - println!("Finished moving to initial joint configuration."); - res -} - fn main() -> FrankaResult<()> { let address = CommandLineArguments::parse(); match address.panda { true => { let robot = Panda::new(address.franka_ip.as_str(), None, None)?; - dyn_generate_motion(robot) + generate_motion(robot) } false => { let mut robot = FR3::new(address.franka_ip.as_str(), None, None)?; - let mut count = 0; - println!("{:?}", robot.read_once()?); - Ok(()) - // robot.read(|robot_state: &RobotState| { - // // Printing to standard output adds a delay. This is acceptable for a read loop such as this, but - // // should not be done in a control loop. - // println!("{:?}", robot_state); - // count += 1; - // count <= 100 - // }); - // dyn_generate_motion(robot) + generate_motion(robot) } } } diff --git a/src/model.rs b/src/model.rs index 0f66aee..d6b04d9 100644 --- a/src/model.rs +++ b/src/model.rs @@ -5,7 +5,7 @@ use nalgebra::Matrix4; use crate::model::model_library::ModelLibrary; -use crate::robot::robot_state::{AbstractRobotState, RobotState}; +use crate::robot::robot_state::RobotState; use crate::FrankaResult; use std::fmt; use std::path::Path; @@ -65,7 +65,6 @@ impl fmt::Display for Frame { } pub trait RobotModel { - type State: AbstractRobotState; fn new>(model_filename: S, libm_filename: Option<&Path>) -> FrankaResult where Self: Sized; @@ -78,7 +77,7 @@ pub trait RobotModel { EE_T_K: &[f64; 16], ) -> [f64; 16]; - fn pose_from_state(&self, frame: &Frame, robot_state: &Self::State) -> [f64; 16]; + fn pose_from_state(&self, frame: &Frame, robot_state: &RobotState) -> [f64; 16]; #[allow(non_snake_case)] fn body_jacobian( &self, @@ -88,7 +87,7 @@ pub trait RobotModel { EE_T_K: &[f64; 16], ) -> [f64; 42]; - fn body_jacobian_from_state(&self, frame: &Frame, robot_state: &Self::State) -> [f64; 42]; + fn body_jacobian_from_state(&self, frame: &Frame, robot_state: &RobotState) -> [f64; 42]; #[allow(non_snake_case)] fn zero_jacobian( &self, @@ -97,7 +96,7 @@ pub trait RobotModel { F_T_EE: &[f64; 16], EE_T_K: &[f64; 16], ) -> [f64; 42]; - fn zero_jacobian_from_state(&self, frame: &Frame, robot_state: &Self::State) -> [f64; 42]; + fn zero_jacobian_from_state(&self, frame: &Frame, robot_state: &RobotState) -> [f64; 42]; #[allow(non_snake_case)] fn mass( &self, @@ -106,7 +105,7 @@ pub trait RobotModel { m_total: f64, F_x_Ctotal: &[f64; 3], ) -> [f64; 49]; - fn mass_from_state(&self, robot_state: &Self::State) -> [f64; 49]; + fn mass_from_state(&self, robot_state: &RobotState) -> [f64; 49]; #[allow(non_snake_case)] fn coriolis( &self, @@ -116,7 +115,7 @@ pub trait RobotModel { m_total: f64, F_x_Ctotal: &[f64; 3], ) -> [f64; 7]; - fn coriolis_from_state(&self, robot_state: &Self::State) -> [f64; 7]; + fn coriolis_from_state(&self, robot_state: &RobotState) -> [f64; 7]; #[allow(non_snake_case)] fn gravity<'a, Grav: Into>>( &self, @@ -128,7 +127,7 @@ pub trait RobotModel { #[allow(non_snake_case)] fn gravity_from_state<'a, Grav: Into>>( &self, - robot_state: &Self::State, + robot_state: &RobotState, gravity_earth: Grav, ) -> [f64; 7]; } @@ -140,7 +139,6 @@ pub struct PandaModel { #[allow(non_snake_case)] impl RobotModel for PandaModel { - type State = RobotState; /// 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 @@ -222,7 +220,7 @@ impl RobotModel for PandaModel { /// * `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: &Self::State) -> [f64; 16] { + fn pose_from_state(&self, frame: &Frame, robot_state: &RobotState) -> [f64; 16] { self.pose( frame, &robot_state.q, @@ -280,7 +278,7 @@ impl RobotModel for PandaModel { /// * `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: &Self::State) -> [f64; 42] { + fn body_jacobian_from_state(&self, frame: &Frame, robot_state: &RobotState) -> [f64; 42] { self.body_jacobian( frame, &robot_state.q, @@ -337,7 +335,7 @@ impl RobotModel for PandaModel { /// * `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: &Self::State) -> [f64; 42] { + fn zero_jacobian_from_state(&self, frame: &Frame, robot_state: &RobotState) -> [f64; 42] { self.zero_jacobian( frame, &robot_state.q, @@ -371,7 +369,7 @@ impl RobotModel for PandaModel { /// * `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: &Self::State) -> [f64; 49] { + fn mass_from_state(&self, robot_state: &RobotState) -> [f64; 49] { self.mass( &robot_state.q, &robot_state.I_total, @@ -410,7 +408,7 @@ impl RobotModel for PandaModel { /// * `robot_state` - State from which the Coriolis force vector should be calculated. /// # Return /// Coriolis force vector. - fn coriolis_from_state(&self, robot_state: &Self::State) -> [f64; 7] { + fn coriolis_from_state(&self, robot_state: &RobotState) -> [f64; 7] { self.coriolis( &robot_state.q, &robot_state.dq, @@ -450,7 +448,7 @@ impl RobotModel for PandaModel { /// Gravity vector. fn gravity_from_state<'a, Grav: Into>>( &self, - robot_state: &Self::State, + robot_state: &RobotState, gravity_earth: Grav, ) -> [f64; 7] { self.gravity( @@ -469,7 +467,6 @@ pub struct FR3Model { #[allow(non_snake_case)] impl RobotModel for FR3Model { - type State = RobotState; /// 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 @@ -551,7 +548,7 @@ impl RobotModel for FR3Model { /// * `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: &Self::State) -> [f64; 16] { + fn pose_from_state(&self, frame: &Frame, robot_state: &RobotState) -> [f64; 16] { self.pose( frame, &robot_state.q, @@ -609,7 +606,7 @@ impl RobotModel for FR3Model { /// * `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: &Self::State) -> [f64; 42] { + fn body_jacobian_from_state(&self, frame: &Frame, robot_state: &RobotState) -> [f64; 42] { self.body_jacobian( frame, &robot_state.q, @@ -666,7 +663,7 @@ impl RobotModel for FR3Model { /// * `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: &Self::State) -> [f64; 42] { + fn zero_jacobian_from_state(&self, frame: &Frame, robot_state: &RobotState) -> [f64; 42] { self.zero_jacobian( frame, &robot_state.q, @@ -700,7 +697,7 @@ impl RobotModel for FR3Model { /// * `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: &Self::State) -> [f64; 49] { + fn mass_from_state(&self, robot_state: &RobotState) -> [f64; 49] { self.mass( &robot_state.q, &robot_state.I_total, @@ -739,7 +736,7 @@ impl RobotModel for FR3Model { /// * `robot_state` - State from which the Coriolis force vector should be calculated. /// # Return /// Coriolis force vector. - fn coriolis_from_state(&self, robot_state: &Self::State) -> [f64; 7] { + fn coriolis_from_state(&self, robot_state: &RobotState) -> [f64; 7] { self.coriolis( &robot_state.q, &robot_state.dq, @@ -779,7 +776,7 @@ impl RobotModel for FR3Model { /// Gravity vector. fn gravity_from_state<'a, Grav: Into>>( &self, - robot_state: &Self::State, + robot_state: &RobotState, gravity_earth: Grav, ) -> [f64; 7] { self.gravity( diff --git a/src/model/library_downloader.rs b/src/model/library_downloader.rs index f2bab21..9f766b3 100644 --- a/src/model/library_downloader.rs +++ b/src/model/library_downloader.rs @@ -3,7 +3,7 @@ use crate::exception::FrankaException::ModelException; use crate::network::{Network, RobotData}; use crate::robot::service_types::{ - LoadModelLibraryArchitecture, LoadModelLibraryRequest, LoadModelLibraryResponse, + LoadModelLibraryArchitecture, LoadModelLibraryRequest, LoadModelLibraryStatus, LoadModelLibrarySystem, }; use crate::FrankaResult; @@ -33,7 +33,7 @@ impl LibraryDownloader for LibraryDownloaderGeneric { 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 _response: LoadModelLibraryResponse = + 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(), diff --git a/src/network.rs b/src/network.rs index f001d7a..c4870d6 100644 --- a/src/network.rs +++ b/src/network.rs @@ -38,13 +38,13 @@ use crate::robot::service_types::{ ConnectRequestWithFR3Header, ConnectRequestWithPandaHeader, FR3CommandEnum, FR3CommandHeader, GetterSetterStatusFR3, GetterSetterStatusPanda, LoadModelLibraryRequest, LoadModelLibraryRequestWithFR3Header, LoadModelLibraryRequestWithPandaHeader, - LoadModelLibraryResponse, LoadModelLibraryStatus, MoveRequest, MoveRequestWithFR3Header, - MoveRequestWithPandaHeader, MoveStatusFR3, MoveStatusPanda, PandaCommandEnum, - PandaCommandHeader, RobotHeader, SetCartesianImpedanceRequest, - SetCartesianImpedanceRequestWithFR3Header, SetCartesianImpedanceRequestWithPandaHeader, - SetCollisionBehaviorRequest, SetCollisionBehaviorRequestWithFR3Header, - SetCollisionBehaviorRequestWithPandaHeader, SetEeToKRequest, SetEeToKRequestWithFR3Header, - SetEeToKRequestWithPandaHeader, SetGuidingModeRequest, SetGuidingModeRequestWithFR3Header, + LoadModelLibraryStatus, MoveRequest, MoveRequestWithFR3Header, MoveRequestWithPandaHeader, + MoveStatusFR3, MoveStatusPanda, PandaCommandEnum, PandaCommandHeader, RobotHeader, + SetCartesianImpedanceRequest, SetCartesianImpedanceRequestWithFR3Header, + SetCartesianImpedanceRequestWithPandaHeader, SetCollisionBehaviorRequest, + SetCollisionBehaviorRequestWithFR3Header, SetCollisionBehaviorRequestWithPandaHeader, + SetEeToKRequest, SetEeToKRequestWithFR3Header, SetEeToKRequestWithPandaHeader, + SetGuidingModeRequest, SetGuidingModeRequestWithFR3Header, SetGuidingModeRequestWithPandaHeader, SetJointImpedanceRequest, SetJointImpedanceRequestWithFR3Header, SetJointImpedanceRequestWithPandaHeader, SetLoadRequest, SetLoadRequestWithFR3Header, SetLoadRequestWithPandaHeader, SetNeToEeRequest, @@ -103,6 +103,7 @@ pub trait RobotData: DeviceData { 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? + // type LoadModelResponse: DeserializeOwned + Copy + Clone + 'static; // todo is this static fine here? fn create_model_library_request( command_id: &mut u32, @@ -875,11 +876,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 { @@ -889,13 +892,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). /// @@ -1025,7 +1029,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, diff --git a/src/robot/robot_impl.rs b/src/robot/robot_impl.rs index 126e4f1..49b75d9 100644 --- a/src/robot/robot_impl.rs +++ b/src/robot/robot_impl.rs @@ -233,7 +233,7 @@ impl> RobotImplementation Ok(Data::State::from(self.receive_robot_state()?)) } fn load_model(&mut self, persistent: bool) -> FrankaResult { - let model_file = Path::new("/tmp/model.so"); + let model_file = Path::new("/tmp/model.so"); // TODO when we load from file we need to distinguish between FR3 and panda let model_already_downloaded = model_file.exists(); if !model_already_downloaded { LibraryDownloaderGeneric::::download(&mut self.network, model_file)?; diff --git a/src/robot/service_types.rs b/src/robot/service_types.rs index ed244d2..cf489d6 100644 --- a/src/robot/service_types.rs +++ b/src/robot/service_types.rs @@ -745,12 +745,6 @@ define_fr3_request_with_header!( // } // } -#[derive(Serialize, Deserialize, Debug)] -pub struct LoadModelLibraryResponse { - pub header: PandaCommandHeader, - pub status: LoadModelLibraryStatus, -} - #[derive(Serialize, Deserialize, Debug, Copy, Clone)] #[repr(packed)] pub struct PandaCommandHeader { From 90623e1f29a60de7bba1730e808e7148dc86995a Mon Sep 17 00:00:00 2001 From: Marco Boneberger Date: Fri, 21 Apr 2023 10:56:16 +0200 Subject: [PATCH 14/63] make load_model work with RobotWrapper trait --- examples/cartesian_impedance_control.rs | 15 +-- examples/generate_cartesian_pose_motion.rs | 11 +- src/robot.rs | 141 ++++++++++++--------- 3 files changed, 90 insertions(+), 77 deletions(-) diff --git a/examples/cartesian_impedance_control.rs b/examples/cartesian_impedance_control.rs index 517fb34..ff68a9d 100644 --- a/examples/cartesian_impedance_control.rs +++ b/examples/cartesian_impedance_control.rs @@ -3,7 +3,7 @@ use clap::Parser; use franka::robot::robot_state::RobotState; -use franka::robot::{Panda, Robot, RobotWrapper, FR3}; +use franka::robot::{Panda, RobotWrapper, FR3}; use franka::FrankaResult; use franka::Torques; use franka::{array_to_isometry, Matrix6x7, Vector7}; @@ -27,7 +27,8 @@ struct CommandLineArguments { pub panda: bool, } -fn generate_motion(mut robot: R, model: M) -> FrankaResult<()> { +fn generate_motion(mut robot: R) -> FrankaResult<()> { + let model = robot.load_model(false)?; let translational_stiffness = 150.; let rotational_stiffness = 10.; @@ -117,14 +118,12 @@ fn main() -> FrankaResult<()> { let address = CommandLineArguments::parse(); match address.panda { true => { - let mut robot = Panda::new(address.franka_ip.as_str(), None, None)?; - let model = robot.load_model(false).unwrap(); - generate_motion(robot, model) + let robot = Panda::new(address.franka_ip.as_str(), None, None)?; + generate_motion(robot) } false => { - let mut robot = FR3::new(address.franka_ip.as_str(), None, None)?; - let model = robot.load_model(false).unwrap(); - generate_motion(robot, model) + let robot = FR3::new(address.franka_ip.as_str(), None, None)?; + generate_motion(robot) } } } diff --git a/examples/generate_cartesian_pose_motion.rs b/examples/generate_cartesian_pose_motion.rs index fe36379..33bc179 100644 --- a/examples/generate_cartesian_pose_motion.rs +++ b/examples/generate_cartesian_pose_motion.rs @@ -3,7 +3,7 @@ use clap::Parser; use franka::model::RobotModel; -use franka::robot::{Robot, RobotWrapper, FR3}; +use franka::robot::{RobotWrapper, FR3}; use franka::{ CartesianPose, CartesianVelocities, ConvertMotion, Frame, JointPositions, JointVelocities, Panda, RobotData, RobotState, @@ -26,14 +26,7 @@ struct CommandLineArguments { pub panda: bool, } -fn generate_motion(mut robot: R) -> FrankaResult<()> -where - CartesianPose: ConvertMotion<<::Data as RobotData>::State>, - CartesianVelocities: ConvertMotion<<::Data as RobotData>::State>, - JointPositions: ConvertMotion<<::Data as RobotData>::State>, - JointVelocities: ConvertMotion<<::Data as RobotData>::State>, - RobotState: From<<::Data as RobotData>::State>, -{ +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/src/robot.rs b/src/robot.rs index 51d313d..0efb09b 100644 --- a/src/robot.rs +++ b/src/robot.rs @@ -27,7 +27,7 @@ use crate::robot::service_types::{ }; use crate::robot::virtual_wall_cuboid::VirtualWallCuboid; use crate::utils::MotionGenerator; -use crate::{Finishable, RobotState}; +use crate::{FR3Model, Finishable, PandaModel, RobotModel, RobotState}; mod control_loop; mod control_tools; @@ -46,6 +46,8 @@ pub(crate) mod types; pub mod virtual_wall_cuboid; pub trait RobotWrapper { + type Model: RobotModel; + /// Starts a loop for reading the current robot state. /// /// Cannot be executed while a control or motion generator loop is running. @@ -575,9 +577,24 @@ pub trait RobotWrapper { 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 + /// 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. + fn load_model(&mut self, persistent: bool) -> FrankaResult; + + /// Returns the software version reported by the connected server. + /// + /// # Return + /// Software version of the connected server. + fn server_version(&self) -> u16; } -impl RobotWrapper for R +impl RobotWrapper for R where RobotState: From<<::Data as RobotData>::State>, CartesianPose: control_types::ConvertMotion<<::Data as RobotData>::State>, @@ -585,9 +602,11 @@ where JointPositions: control_types::ConvertMotion<<::Data as RobotData>::State>, CartesianVelocities: control_types::ConvertMotion<<::Data as RobotData>::State>, { + type Model = <::Data as RobotData>::Model; + fn read bool>(&mut self, mut read_callback: F) -> FrankaResult<()> { loop { - let state = ::get_rob_mut(self).update(None, None)?; + let state = ::get_rob_mut(self).update(None, None)?; if !read_callback(&state.into()) { break; } @@ -610,7 +629,7 @@ where let cb = |state: &<::Data as RobotData>::State, duration: &Duration| { motion_generator_callback(&(state.clone().into()), duration) }; - ::control_motion_intern( + ::control_motion_intern( self, cb, controller_mode.into(), @@ -634,7 +653,7 @@ where let cb = |state: &<::Data as RobotData>::State, duration: &Duration| { motion_generator_callback(&(state.clone().into()), duration) // todo make this without cloning }; - ::control_motion_intern( + ::control_motion_intern( self, cb, controller_mode.into(), @@ -658,7 +677,7 @@ where let cb = |state: &<::Data as RobotData>::State, duration: &Duration| { motion_generator_callback(&(state.clone().into()), duration) }; - ::control_motion_intern( + ::control_motion_intern( self, cb, controller_mode.into(), @@ -838,7 +857,7 @@ where } fn read_once(&mut self) -> FrankaResult { - match ::get_rob_mut(self).read_once() { + match ::get_rob_mut(self).read_once() { Ok(state) => Ok(state.into()), Err(e) => Err(e), } @@ -857,7 +876,7 @@ where upper_force_thresholds_nominal: [f64; 6], ) -> FrankaResult<()> { let command = ::Data::create_set_collision_behavior_request( - &mut ::get_net(self).command_id, + &mut ::get_net(self).command_id, SetCollisionBehaviorRequest::new( lower_torque_thresholds_acceleration, upper_torque_thresholds_acceleration, @@ -869,8 +888,8 @@ where 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); + let command_id: u32 = ::get_net(self).tcp_send_request(command); + let status = ::get_net(self).tcp_blocking_receive_status(command_id); ::Data::handle_getter_setter_status(status) } @@ -886,22 +905,22 @@ where #[allow(non_snake_case)] fn set_joint_impedance(&mut self, K_theta: [f64; 7]) -> FrankaResult<()> { let command = ::Data::create_set_joint_impedance_request( - &mut ::get_net(self).command_id, + &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); + let command_id: u32 = ::get_net(self).tcp_send_request(command); + let status = ::get_net(self).tcp_blocking_receive_status(command_id); ::Data::handle_getter_setter_status(status) } #[allow(non_snake_case)] fn set_cartesian_impedance(&mut self, K_x: [f64; 6]) -> FrankaResult<()> { let command = ::Data::create_set_cartesian_impedance_request( - &mut ::get_net(self).command_id, + &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); + let command_id: u32 = ::get_net(self).tcp_send_request(command); + let status = ::get_net(self).tcp_blocking_receive_status(command_id); ::Data::handle_getter_setter_status(status) } @@ -913,62 +932,62 @@ where load_inertia: [f64; 9], ) -> FrankaResult<()> { let command = ::Data::create_set_load_request( - &mut ::get_net(self).command_id, + &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); + let command_id: u32 = ::get_net(self).tcp_send_request(command); + let status = ::get_net(self).tcp_blocking_receive_status(command_id); ::Data::handle_getter_setter_status(status) } fn set_guiding_mode(&mut self, guiding_mode: [bool; 6], elbow: bool) -> FrankaResult<()> { let command = ::Data::create_set_guiding_mode_request( - &mut ::get_net(self).command_id, + &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); + let command_id: u32 = ::get_net(self).tcp_send_request(command); + let status = ::get_net(self).tcp_blocking_receive_status(command_id); ::Data::handle_getter_setter_status(status) } #[allow(non_snake_case)] fn set_K(&mut self, EE_T_K: [f64; 16]) -> FrankaResult<()> { let command = ::Data::create_set_ee_to_k_request( - &mut ::get_net(self).command_id, + &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); + let command_id: u32 = ::get_net(self).tcp_send_request(command); + let status = ::get_net(self).tcp_blocking_receive_status(command_id); ::Data::handle_getter_setter_status(status) } #[allow(non_snake_case)] fn set_EE(&mut self, NE_T_EE: [f64; 16]) -> FrankaResult<()> { let command = ::Data::create_set_ne_to_ee_request( - &mut ::get_net(self).command_id, + &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); + let command_id: u32 = ::get_net(self).tcp_send_request(command); + let status = ::get_net(self).tcp_blocking_receive_status(command_id); ::Data::handle_getter_setter_status(status) } fn automatic_error_recovery(&mut self) -> FrankaResult<()> { let command = ::Data::create_automatic_error_recovery_request( - &mut ::get_net(self).command_id, + &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); + let command_id: u32 = ::get_net(self).tcp_send_request(command); + let status = ::get_net(self).tcp_blocking_receive_status(command_id); ::Data::handle_automatic_error_recovery_status(status) } fn stop(&mut self) -> FrankaResult<()> { let command = ::Data::create_stop_request( - &mut ::get_net(self).command_id, + &mut ::get_net(self).command_id, ); - let command_id: u32 = ::get_net(self).tcp_send_request(command); + let command_id: u32 = ::get_net(self).tcp_send_request(command); let status: StopMoveStatusPanda = - ::get_net(self).tcp_blocking_receive_status(command_id); + ::get_net(self).tcp_blocking_receive_status(command_id); match status { StopMoveStatusPanda::Success => Ok(()), StopMoveStatusPanda::CommandNotPossibleRejected | StopMoveStatusPanda::Aborted => { @@ -984,6 +1003,14 @@ where )), } } + + fn load_model(&mut self, persistent: bool) -> FrankaResult { + ::get_rob_mut(self).load_model(persistent) + } + + fn server_version(&self) -> u16 { + ::get_rob(self).server_version() + } } // impl RobotWrapper for FR3 { @@ -998,7 +1025,7 @@ where // } // } -pub trait Robot +trait PrivateRobot: Robot where CartesianPose: ConvertMotion<<::Data as RobotData>::State>, JointVelocities: ConvertMotion<<::Data as RobotData>::State>, @@ -1006,8 +1033,6 @@ where CartesianVelocities: ConvertMotion<<::Data as RobotData>::State>, RobotState: From<<::Data as RobotData>::State>, { - type Data: RobotData; - 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; @@ -1065,26 +1090,18 @@ where )?; control_loop.run() } +} - /// 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. - fn load_model(&mut self, persistent: bool) -> FrankaResult<::Model> { - self.get_rob_mut().load_model(persistent) - } - - /// Returns the software version reported by the connected server. - /// - /// # Return - /// Software version of the connected server. - fn server_version(&self) -> u16 { - self.get_rob().server_version() - } +pub trait Robot +where + CartesianPose: ConvertMotion<<::Data as RobotData>::State>, + JointVelocities: ConvertMotion<<::Data as RobotData>::State>, + JointPositions: ConvertMotion<<::Data as RobotData>::State>, + CartesianVelocities: ConvertMotion<<::Data as RobotData>::State>, + RobotState: From<<::Data as RobotData>::State>, +{ + type Data: RobotData; + type Rob: RobotImplementation; } /// Maintains a network connection to the robot, provides the current robot state, gives access to @@ -1147,6 +1164,9 @@ pub struct Panda { impl Robot for Panda { type Data = PandaData; type Rob = RobotImplGeneric; +} + +impl PrivateRobot for Panda { fn get_net(&mut self) -> &mut Network { &mut self.robimpl.network } @@ -1162,10 +1182,7 @@ impl Robot for Panda { pub struct FR3 { robimpl: RobotImplGeneric, } - -impl Robot for FR3 { - type Data = FR3Data; - type Rob = RobotImplGeneric; +impl PrivateRobot for FR3 { fn get_net(&mut self) -> &mut Network { &mut self.robimpl.network } @@ -1177,6 +1194,10 @@ impl Robot for FR3 { &self.robimpl } } +impl Robot for FR3 { + type Data = FR3Data; + type Rob = RobotImplGeneric; +} impl FR3 { pub fn new>, LogSize: Into>>( From ec9c6ec94332c667c480e4af616f2e175efcfb7c Mon Sep 17 00:00:00 2001 From: Marco Boneberger Date: Fri, 21 Apr 2023 10:58:39 +0200 Subject: [PATCH 15/63] remove unnecessary braces in rate_limiting.rs --- src/robot/rate_limiting.rs | 20 ++++++-------------- 1 file changed, 6 insertions(+), 14 deletions(-) diff --git a/src/robot/rate_limiting.rs b/src/robot/rate_limiting.rs index a621576..e924413 100644 --- a/src/robot/rate_limiting.rs +++ b/src/robot/rate_limiting.rs @@ -317,10 +317,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 +335,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 +343,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 @@ -418,10 +410,10 @@ pub fn limit_rate_cartesian_velocity( 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 } From e5ebea4f88eee202f96555da77f9a5bbcff8409e Mon Sep 17 00:00:00 2001 From: Marco Boneberger Date: Fri, 21 Apr 2023 11:23:55 +0200 Subject: [PATCH 16/63] minor improvements --- examples/cartesian_impedance_control.rs | 28 ++++++++++++------------- src/robot.rs | 13 +----------- 2 files changed, 15 insertions(+), 26 deletions(-) diff --git a/examples/cartesian_impedance_control.rs b/examples/cartesian_impedance_control.rs index ff68a9d..fc34342 100644 --- a/examples/cartesian_impedance_control.rs +++ b/examples/cartesian_impedance_control.rs @@ -27,6 +27,20 @@ struct CommandLineArguments { pub panda: bool, } +fn main() -> FrankaResult<()> { + 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 model = robot.load_model(false)?; let translational_stiffness = 150.; @@ -113,17 +127,3 @@ fn generate_motion(mut robot: R) -> FrankaResult<()> { } } } - -fn main() -> FrankaResult<()> { - 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) - } - } -} diff --git a/src/robot.rs b/src/robot.rs index 0efb09b..3d3aca0 100644 --- a/src/robot.rs +++ b/src/robot.rs @@ -577,6 +577,7 @@ pub trait RobotWrapper { 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` @@ -1013,18 +1014,6 @@ where } } -// impl RobotWrapper for FR3 { -// fn read bool>(&mut self, mut read_callback: F) -> FrankaResult<()> { -// loop { -// let state = self.get_rob_mut().update(None, None)?; -// if !read_callback(&state.into()) { -// break; -// } -// } -// Ok(()) -// } -// } - trait PrivateRobot: Robot where CartesianPose: ConvertMotion<<::Data as RobotData>::State>, From 2450772a2bb3690d6eb032c23ad1486fd83db218 Mon Sep 17 00:00:00 2001 From: Marco Boneberger Date: Fri, 21 Apr 2023 13:36:56 +0200 Subject: [PATCH 17/63] make all examples work again --- examples/cartesian_impedance_control.rs | 22 +++---- examples/communication_test.rs | 27 +++++++-- examples/download_model.rs | 26 ++++++-- examples/echo_robot_state.rs | 23 +++++-- examples/generate_cartesian_pose_motion.rs | 40 ++++++------- .../generate_cartesian_velocity_motion.rs | 27 +++++++-- examples/generate_consecutive_motions.rs | 28 +++++++-- examples/generate_elbow_motion.rs | 26 ++++++-- examples/generate_joint_position_motion.rs | 26 ++++++-- examples/generate_joint_velocity_motion.rs | 28 ++++++--- examples/grasp_object.rs | 4 +- examples/mirror_robot.rs | 60 +++++++++++++++---- examples/print_joint_poses.rs | 21 ++++++- src/lib.rs | 2 + src/robot.rs | 2 +- 15 files changed, 269 insertions(+), 93 deletions(-) diff --git a/examples/cartesian_impedance_control.rs b/examples/cartesian_impedance_control.rs index fc34342..8c65dcf 100644 --- a/examples/cartesian_impedance_control.rs +++ b/examples/cartesian_impedance_control.rs @@ -1,15 +1,15 @@ // Copyright (c) 2021 Marco Boneberger // Licensed under the EUPL-1.2-or-later +use std::time::Duration; + use clap::Parser; -use franka::robot::robot_state::RobotState; -use franka::robot::{Panda, RobotWrapper, FR3}; -use franka::FrankaResult; -use franka::Torques; -use franka::{array_to_isometry, Matrix6x7, Vector7}; -use franka::{Frame, RobotModel}; use nalgebra::{Matrix3, Matrix6, Matrix6x1, UnitQuaternion, Vector3, U1, U3}; -use std::time::Duration; + +use franka::{ + array_to_isometry, Frame, FrankaResult, Matrix6x7, Panda, RobotModel, RobotState, RobotWrapper, + Torques, Vector7, FR3, +}; /// An example showing a simple cartesian impedance controller without inertia shaping /// that renders a spring damper system where the equilibrium is the initial configuration. @@ -28,14 +28,14 @@ struct CommandLineArguments { } fn main() -> FrankaResult<()> { - let address = CommandLineArguments::parse(); - match address.panda { + let args = CommandLineArguments::parse(); + match args.panda { true => { - let robot = Panda::new(address.franka_ip.as_str(), None, None)?; + let robot = Panda::new(args.franka_ip.as_str(), None, None)?; generate_motion(robot) } false => { - let robot = FR3::new(address.franka_ip.as_str(), None, None)?; + let robot = FR3::new(args.franka_ip.as_str(), None, None)?; generate_motion(robot) } } diff --git a/examples/communication_test.rs b/examples/communication_test.rs index f39e943..a3b5f11 100644 --- a/examples/communication_test.rs +++ b/examples/communication_test.rs @@ -1,12 +1,12 @@ // Copyright (c) 2021 Marco Boneberger // Licensed under the EUPL-1.2-or-later -use clap::Parser; -use franka::robot::{Robot, RobotWrapper, FR3}; -use franka::Torques; -use franka::{Finishable, FrankaResult, RobotState}; use std::f64::consts::PI; use std::time::Duration; +use clap::Parser; + +use franka::{Finishable, FrankaResult, Panda, RobotState, RobotWrapper, Torques, FR3}; + /// An example indicating the network performance. /// /// WARNING: Before executing this example, make sure there is enough space in front of the robot. @@ -15,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 = FR3::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 f63e40f..1011eeb 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::{Robot, FR3}; -use franka::{FrankaResult, RealtimeConfig}; use std::fs; use std::path::PathBuf; +use clap::Parser; + +use franka::exception::FrankaException::ModelException; +use franka::{FrankaResult, Panda, RealtimeConfig, RobotWrapper, FR3}; + /// Downloads the model for offline usage #[derive(Parser, Debug)] #[clap(author, version, name = "download_model")] @@ -18,13 +19,28 @@ struct CommandLineArguments { /// 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 = FR3::new(args.franka_ip.as_str(), RealtimeConfig::Ignore, None)?; + 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, path: PathBuf) -> FrankaResult<()> { robot.load_model(true)?; fs::copy("/tmp/model.so", &path).map_err(|_| ModelException { message: "Could copy model to download location".to_string(), diff --git a/examples/echo_robot_state.rs b/examples/echo_robot_state.rs index 19867fe..4acff88 100644 --- a/examples/echo_robot_state.rs +++ b/examples/echo_robot_state.rs @@ -2,8 +2,8 @@ // Licensed under the EUPL-1.2-or-later use clap::Parser; -use franka::robot::{Robot, RobotWrapper, FR3}; -use franka::{FrankaResult, RobotState}; + +use franka::{FrankaResult, Panda, RobotState, RobotWrapper, FR3}; /// An example showing how to continuously read the robot state. #[derive(Parser, Debug)] @@ -11,11 +11,26 @@ use franka::{FrankaResult, 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 = FR3::new("172.116.0.5", 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<()> { robot.set_collision_behavior( [0.; 7], [0.; 7], [0.; 7], [0.; 7], [0.; 6], [0.; 6], [0.; 6], [0.; 6], )?; diff --git a/examples/generate_cartesian_pose_motion.rs b/examples/generate_cartesian_pose_motion.rs index 33bc179..e9bcd40 100644 --- a/examples/generate_cartesian_pose_motion.rs +++ b/examples/generate_cartesian_pose_motion.rs @@ -1,17 +1,13 @@ // Copyright (c) 2021 Marco Boneberger // Licensed under the EUPL-1.2-or-later -use clap::Parser; -use franka::model::RobotModel; -use franka::robot::{RobotWrapper, FR3}; -use franka::{ - CartesianPose, CartesianVelocities, ConvertMotion, Frame, JointPositions, JointVelocities, - Panda, RobotData, RobotState, -}; -use franka::{Finishable, FrankaResult}; use std::f64::consts::PI; use std::time::Duration; +use clap::Parser; + +use franka::{CartesianPose, Finishable, FrankaResult, Panda, RobotState, RobotWrapper, FR3}; + /// 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. @@ -26,6 +22,20 @@ struct CommandLineArguments { 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<()> { robot.set_default_behavior()?; println!("WARNING: This example will move the robot! Please make sure to have the user stop button at hand!"); @@ -70,17 +80,3 @@ fn generate_motion(mut robot: R) -> FrankaResult<()> { }; robot.control_cartesian_pose(callback, None, None, None) } - -fn main() -> FrankaResult<()> { - let address = CommandLineArguments::parse(); - match address.panda { - true => { - let robot = Panda::new(address.franka_ip.as_str(), None, None)?; - generate_motion(robot) - } - false => { - let mut robot = FR3::new(address.franka_ip.as_str(), None, None)?; - generate_motion(robot) - } - } -} diff --git a/examples/generate_cartesian_velocity_motion.rs b/examples/generate_cartesian_velocity_motion.rs index 21c3b6f..a660816 100644 --- a/examples/generate_cartesian_velocity_motion.rs +++ b/examples/generate_cartesian_velocity_motion.rs @@ -1,13 +1,13 @@ // Copyright (c) 2021 Marco Boneberger // Licensed under the EUPL-1.2-or-later -use clap::Parser; -use franka::robot::robot_state::RobotState; -use franka::robot::{Robot, RobotWrapper, FR3}; -use franka::{CartesianVelocities, Finishable, FrankaResult}; use std::f64::consts::PI; use std::time::Duration; +use clap::Parser; + +use franka::{CartesianVelocities, Finishable, FrankaResult, Panda, RobotState, RobotWrapper, FR3}; + /// 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. @@ -16,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 = FR3::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 bc3a461..293007b 100644 --- a/examples/generate_consecutive_motions.rs +++ b/examples/generate_consecutive_motions.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; -use franka::robot::{Robot, RobotWrapper, FR3}; -use franka::{Finishable, FrankaResult, JointVelocities, RobotState}; use std::f64::consts::PI; use std::time::Duration; +use clap::Parser; + +use franka::exception::FrankaException; +use franka::{Finishable, FrankaResult, JointVelocities, Panda, RobotState, RobotWrapper, FR3}; + /// 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 @@ -17,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 = FR3::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 38b8055..d69a2c0 100644 --- a/examples/generate_elbow_motion.rs +++ b/examples/generate_elbow_motion.rs @@ -1,12 +1,13 @@ // Copyright (c) 2021 Marco Boneberger // Licensed under the EUPL-1.2-or-later -use clap::Parser; -use franka::robot::{Robot, RobotWrapper, FR3}; -use franka::{CartesianPose, Finishable, FrankaResult, RobotState}; use std::f64::consts::PI; use std::time::Duration; +use clap::Parser; + +use franka::{CartesianPose, Finishable, FrankaResult, Panda, RobotState, RobotWrapper, FR3}; + /// 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. @@ -15,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 = FR3::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 1e63f3e..de21dc9 100644 --- a/examples/generate_joint_position_motion.rs +++ b/examples/generate_joint_position_motion.rs @@ -1,12 +1,13 @@ // Copyright (c) 2021 Marco Boneberger // Licensed under the EUPL-1.2-or-later -use clap::Parser; -use franka::robot::{Robot, RobotWrapper, FR3}; -use franka::{Finishable, FrankaResult, JointPositions, RobotState}; use std::f64::consts::PI; use std::time::Duration; +use clap::Parser; + +use franka::{Finishable, FrankaResult, JointPositions, Panda, RobotState, RobotWrapper, FR3}; + /// 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. @@ -15,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 = FR3::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 4a4d016..55718ca 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::robot::{Robot, RobotWrapper, FR3}; -use franka::JointVelocities; -use franka::RobotState; -use franka::{Finishable, FrankaResult}; use std::f64::consts::PI; use std::time::Duration; +use clap::Parser; + +use franka::{Finishable, FrankaResult, JointVelocities, Panda, RobotState, RobotWrapper, FR3}; + /// 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 = FR3::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..05b00cc 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)] diff --git a/examples/mirror_robot.rs b/examples/mirror_robot.rs index bbbbd80..a626d29 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, RobotWrapper, FR3}; -use franka::utils::{array_to_isometry, Matrix6x7, Vector7}; -use franka::RobotState; -use franka::{Matrix7, RobotModel}; -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, U1, U3}; + +use franka::{ + array_to_isometry, Frame, FrankaResult, Matrix6x7, Matrix7, Panda, RobotModel, RobotState, + RobotWrapper, Torques, Vector7, FR3, +}; + /// 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,11 +27,51 @@ struct CommandLineArguments { /// IP-Address or hostname of the robot which mirrors the movement #[clap(long)] pub franka_ip_mirror: String, + + /// Use this option to run the example on a Panda + #[clap(short, long, action)] + pub panda_user: bool, + + /// Use this option to run the example on a Panda + #[clap(short, 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.; @@ -50,8 +90,6 @@ fn main() -> FrankaResult<()> { bottom_right_corner .copy_from(&(2. * f64::sqrt(rotational_stiffness) * Matrix3::identity())); } - let mut robot_user = FR3::new(args.franka_ip_user.as_str(), None, None)?; - let mut robot_mirror = FR3::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], diff --git a/examples/print_joint_poses.rs b/examples/print_joint_poses.rs index 9e06ddb..a6704d1 100644 --- a/examples/print_joint_poses.rs +++ b/examples/print_joint_poses.rs @@ -1,10 +1,10 @@ // Copyright (c) 2021 Marco Boneberger // Licensed under the EUPL-1.2-or-later use clap::Parser; -use franka::robot::{Robot, RobotWrapper, FR3}; -use franka::{Frame, FrankaResult, RealtimeConfig, RobotModel}; use nalgebra::Matrix4; +use franka::{Frame, FrankaResult, Panda, RobotModel, RobotWrapper, FR3}; + /// 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)] @@ -12,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 = FR3::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/lib.rs b/src/lib.rs index 1111b4a..c11c6ef 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -184,4 +184,6 @@ 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::Panda; +pub use robot::RobotWrapper; +pub use robot::FR3; pub use utils::*; diff --git a/src/robot.rs b/src/robot.rs index 3d3aca0..b9c6da3 100644 --- a/src/robot.rs +++ b/src/robot.rs @@ -27,7 +27,7 @@ use crate::robot::service_types::{ }; use crate::robot::virtual_wall_cuboid::VirtualWallCuboid; use crate::utils::MotionGenerator; -use crate::{FR3Model, Finishable, PandaModel, RobotModel, RobotState}; +use crate::{Finishable, RobotModel, RobotState}; mod control_loop; mod control_tools; From fef13bbd701f4775cbf09997076f5a5e1a74e6a5 Mon Sep 17 00:00:00 2001 From: Marco Boneberger Date: Fri, 21 Apr 2023 13:39:55 +0200 Subject: [PATCH 18/63] make clippy happy --- src/network.rs | 8 ++++---- src/robot/logger.rs | 2 +- src/robot/robot_impl.rs | 6 +++--- src/robot/types.rs | 9 ++------- 4 files changed, 10 insertions(+), 15 deletions(-) diff --git a/src/network.rs b/src/network.rs index c4870d6..dbda7f5 100644 --- a/src/network.rs +++ b/src/network.rs @@ -943,7 +943,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); @@ -1045,7 +1045,7 @@ impl Network { 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; + 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(); @@ -1054,13 +1054,13 @@ impl Network { 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(); diff --git a/src/robot/logger.rs b/src/robot/logger.rs index 2b03b7c..7d2354a 100644 --- a/src/robot/logger.rs +++ b/src/robot/logger.rs @@ -43,7 +43,7 @@ pub struct Record { impl Record { /// creates a string representation based on the debug formatter pub fn log(&self) -> String { - format!("{:?}", self.clone()) + format!("{:?}", self) } } diff --git a/src/robot/robot_impl.rs b/src/robot/robot_impl.rs index 49b75d9..2567cf7 100644 --- a/src/robot/robot_impl.rs +++ b/src/robot/robot_impl.rs @@ -136,7 +136,7 @@ impl RobotControl for RobotImplGeneric { 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(), + robot_state.get_last_motion_errors(), self.logger.flush(), )?; @@ -146,7 +146,7 @@ impl RobotControl for RobotImplGeneric { return Err(Data::create_control_exception( message, status, - &robot_state.get_last_motion_errors(), + robot_state.get_last_motion_errors(), self.logger.flush(), )); } @@ -211,7 +211,7 @@ impl RobotControl for RobotImplGeneric { Err(error) => Err(Data::create_control_exception( error.to_string(), status, - &robot_state.get_last_motion_errors(), + robot_state.get_last_motion_errors(), self.logger.flush(), )), Ok(_) => panic!("Unexpected reply to a Move command"), diff --git a/src/robot/types.rs b/src/robot/types.rs index 59e4601..755cae7 100644 --- a/src/robot/types.rs +++ b/src/robot/types.rs @@ -2,7 +2,6 @@ // 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 +25,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,11 +37,6 @@ pub enum RobotMode { UserStopped, AutomaticErrorRecovery, } -impl Default for RobotMode { - fn default() -> Self { - Other - } -} #[derive(Serialize, Deserialize, Debug, Copy, Clone, PartialEq)] #[allow(non_snake_case)] From de81aecbce1f51882942171eef99177ac248bc0e Mon Sep 17 00:00:00 2001 From: Marco Boneberger Date: Fri, 21 Apr 2023 13:43:12 +0200 Subject: [PATCH 19/63] delete some commented out code --- src/network.rs | 51 +---------------------------------------- src/robot/robot_impl.rs | 46 ------------------------------------- 2 files changed, 1 insertion(+), 96 deletions(-) diff --git a/src/network.rs b/src/network.rs index dbda7f5..86831f3 100644 --- a/src/network.rs +++ b/src/network.rs @@ -234,19 +234,6 @@ impl RobotData for PandaData { type DeviceData = Self; type StateIntern = PandaStateIntern; type Model = PandaModel; - - // fn create_model_library_request( - // mut command_id: &mut u32, - // request: LoadModelLibraryRequest, - // ) -> Self::LoadModelRequestWithHeader { - // let header = Self::create_header( - // &mut command_id, - // PandaCommandEnum::LoadModelLibrary, - // size_of::(), - // ); - // Self::LoadModelRequestWithHeader { header, request } - // } - type Header = PandaCommandHeader; type LoadModelRequestWithHeader = LoadModelLibraryRequestWithPandaHeader; type SetCollisionBehaviorRequestWithHeader = SetCollisionBehaviorRequestWithPandaHeader; @@ -679,25 +666,6 @@ impl RobotData for FR3Data { )), } } - - // fn create_model_library_request( - // mut command_id: &mut u32, - // request: LoadModelLibraryRequest, - // ) -> Self::LoadModelRequestWithHeader - // where - // <::DeviceData as DeviceData>::CommandHeader: RobotHeader, - // { - // // let header = Self::create_header( - // // &mut command_id, - // // FR3CommandEnum::LoadModelLibrary, - // // size_of::(), - // // ); - // // Self::LoadModelRequestWithHeader { - // // header, - // // request, - // // } - // (command_id,request).into() - // } } impl DeviceData for GripperData { @@ -795,15 +763,7 @@ impl Network { data: PhantomData, }) } - // pub fn create_header_for_gripper( - // &mut self, - // command: gripper::types::GripperCommandEnum, - // size: usize, - // ) -> GripperCommandHeader { - // let header = gripper::types::GripperCommandHeader::new(command, self.command_id, size as u32); - // self.command_id += 1; - // header - // } + pub fn create_header_for_panda( &mut self, command: PandaCommandEnum, @@ -821,15 +781,6 @@ impl Network { ) -> Data::CommandHeader { Data::create_header(&mut self.command_id, command, size) } - // pub fn create_header_for_fr3( - // &mut self, - // command: FR3CommandEnum, - // size: usize, - // ) -> FR3CommandHeader { - // let header = FR3CommandHeader::new(command, self.command_id, size as u32); - // self.command_id += 1; - // header - // } pub fn tcp_send_request(&mut self, request: T) -> u32 { let encoded_request = serialize(&request); diff --git a/src/robot/robot_impl.rs b/src/robot/robot_impl.rs index 2567cf7..b9e5ce5 100644 --- a/src/robot/robot_impl.rs +++ b/src/robot/robot_impl.rs @@ -250,40 +250,6 @@ impl> RobotImplementation } } -// fn handle_command_move_status(status: &MoveStatusPanda) -> 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!", -// )), -// } -// } - impl RobotImplGeneric { pub fn new( network: Network, @@ -431,18 +397,6 @@ impl RobotImplGeneric { maximum_path_deviation: &MoveDeviation, maximum_goal_deviation: &MoveDeviation, ) -> FrankaResult { - // let connect_command = MoveRequestWithPandaHeader { - // header: self.network.create_header_for_panda( - // PandaCommandEnum::Move, - // size_of::(), - // ), - // request: MoveRequest::new( - // *controller_mode, - // *motion_generator_mode, - // *maximum_path_deviation, - // *maximum_goal_deviation, - // ), - // }; let command = Data::create_move_request( &mut self.network.command_id, MoveRequest::new( From 1cb87b14855d4bb728cc95eb6f98bcfbe063fd0a Mon Sep 17 00:00:00 2001 From: Marco Boneberger Date: Fri, 21 Apr 2023 13:51:45 +0200 Subject: [PATCH 20/63] delete some commented out code --- src/exception.rs | 12 ------------ src/robot.rs | 2 +- 2 files changed, 1 insertion(+), 13 deletions(-) diff --git a/src/exception.rs b/src/exception.rs index 6892705..b15402c 100644 --- a/src/exception.rs +++ b/src/exception.rs @@ -67,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/robot.rs b/src/robot.rs index b9c6da3..5549219 100644 --- a/src/robot.rs +++ b/src/robot.rs @@ -1347,7 +1347,7 @@ mod tests { SetterResponseFR3, COMMAND_PORT, FR3_VERSION, }; use crate::robot::types::PandaStateIntern; - use crate::robot::{Robot, RobotWrapper, FR3}; + use crate::robot::{RobotWrapper, FR3}; use crate::{Finishable, FrankaResult, JointPositions, Panda, RealtimeConfig, RobotState}; use bincode::{deserialize, serialize, serialized_size}; use std::iter::FromIterator; From c959a1e448d77fe3ff80d80f0c1516525cbc21c6 Mon Sep 17 00:00:00 2001 From: Marco Boneberger Date: Fri, 21 Apr 2023 14:48:19 +0200 Subject: [PATCH 21/63] fix ConvertMotion, MotionFinished and spin_control --- examples/communication_test.rs | 2 +- examples/generate_cartesian_pose_motion.rs | 2 +- .../generate_cartesian_velocity_motion.rs | 4 +- examples/generate_consecutive_motions.rs | 2 +- examples/generate_elbow_motion.rs | 2 +- examples/generate_joint_position_motion.rs | 2 +- examples/generate_joint_velocity_motion.rs | 2 +- src/lib.rs | 18 +- src/robot.rs | 6 +- src/robot/control_loop.rs | 3 +- src/robot/control_types.rs | 309 +----------------- 11 files changed, 36 insertions(+), 316 deletions(-) diff --git a/examples/communication_test.rs b/examples/communication_test.rs index a3b5f11..2c3dac4 100644 --- a/examples/communication_test.rs +++ b/examples/communication_test.rs @@ -5,7 +5,7 @@ use std::time::Duration; use clap::Parser; -use franka::{Finishable, FrankaResult, Panda, RobotState, RobotWrapper, Torques, FR3}; +use franka::{FrankaResult, MotionFinished, Panda, RobotState, RobotWrapper, Torques, FR3}; /// An example indicating the network performance. /// diff --git a/examples/generate_cartesian_pose_motion.rs b/examples/generate_cartesian_pose_motion.rs index e9bcd40..27db22d 100644 --- a/examples/generate_cartesian_pose_motion.rs +++ b/examples/generate_cartesian_pose_motion.rs @@ -6,7 +6,7 @@ use std::time::Duration; use clap::Parser; -use franka::{CartesianPose, Finishable, FrankaResult, Panda, RobotState, RobotWrapper, FR3}; +use franka::{CartesianPose, FrankaResult, MotionFinished, Panda, RobotState, RobotWrapper, FR3}; /// An example showing how to generate a Cartesian motion. /// diff --git a/examples/generate_cartesian_velocity_motion.rs b/examples/generate_cartesian_velocity_motion.rs index a660816..3dcb36e 100644 --- a/examples/generate_cartesian_velocity_motion.rs +++ b/examples/generate_cartesian_velocity_motion.rs @@ -6,7 +6,9 @@ use std::time::Duration; use clap::Parser; -use franka::{CartesianVelocities, Finishable, FrankaResult, Panda, RobotState, RobotWrapper, FR3}; +use franka::{ + CartesianVelocities, FrankaResult, MotionFinished, Panda, RobotState, RobotWrapper, FR3, +}; /// An example showing how to generate a Cartesian velocity motion. /// diff --git a/examples/generate_consecutive_motions.rs b/examples/generate_consecutive_motions.rs index 293007b..0a7c220 100644 --- a/examples/generate_consecutive_motions.rs +++ b/examples/generate_consecutive_motions.rs @@ -7,7 +7,7 @@ use std::time::Duration; use clap::Parser; use franka::exception::FrankaException; -use franka::{Finishable, FrankaResult, JointVelocities, Panda, RobotState, RobotWrapper, FR3}; +use franka::{FrankaResult, JointVelocities, MotionFinished, Panda, RobotState, RobotWrapper, FR3}; /// An example showing how to execute consecutive motions with error recovery. /// diff --git a/examples/generate_elbow_motion.rs b/examples/generate_elbow_motion.rs index d69a2c0..c3be957 100644 --- a/examples/generate_elbow_motion.rs +++ b/examples/generate_elbow_motion.rs @@ -6,7 +6,7 @@ use std::time::Duration; use clap::Parser; -use franka::{CartesianPose, Finishable, FrankaResult, Panda, RobotState, RobotWrapper, FR3}; +use franka::{CartesianPose, FrankaResult, MotionFinished, Panda, RobotState, RobotWrapper, FR3}; /// An example showing how to move the robot's elbow. /// diff --git a/examples/generate_joint_position_motion.rs b/examples/generate_joint_position_motion.rs index de21dc9..e2f0f5d 100644 --- a/examples/generate_joint_position_motion.rs +++ b/examples/generate_joint_position_motion.rs @@ -6,7 +6,7 @@ use std::time::Duration; use clap::Parser; -use franka::{Finishable, FrankaResult, JointPositions, Panda, RobotState, RobotWrapper, FR3}; +use franka::{FrankaResult, JointPositions, MotionFinished, Panda, RobotState, RobotWrapper, FR3}; /// An example showing how to generate a joint position motion. /// diff --git a/examples/generate_joint_velocity_motion.rs b/examples/generate_joint_velocity_motion.rs index 55718ca..8b304af 100644 --- a/examples/generate_joint_velocity_motion.rs +++ b/examples/generate_joint_velocity_motion.rs @@ -6,7 +6,7 @@ use std::time::Duration; use clap::Parser; -use franka::{Finishable, FrankaResult, JointVelocities, Panda, RobotState, RobotWrapper, FR3}; +use franka::{FrankaResult, JointVelocities, MotionFinished, Panda, RobotState, RobotWrapper, FR3}; /// An example showing how to generate a joint velocity motion. /// diff --git a/src/lib.rs b/src/lib.rs index c11c6ef..1393fba 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, Panda, FrankaResult}; +//! use franka::{JointPositions, MotionFinished, RobotWrapper, RobotState, FR3, FrankaResult}; //! fn main() -> FrankaResult<()> { -//! let mut robot = Panda::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], @@ -60,9 +60,9 @@ //!```no_run //! # use std::time::Duration; //! # use std::f64::consts::PI; -//! # use franka::{JointPositions, MotionFinished, RobotState, Panda, 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(()) //! # } //! ``` @@ -80,9 +80,9 @@ //!```no_run //! # use std::time::Duration; //! # use std::f64::consts::PI; -//! # use franka::{JointPositions, MotionFinished, RobotState, Robot, FrankaResult}; +//! # use franka::{JointPositions, MotionFinished, RobotState, FR3, RobotWrapper, 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(()) @@ -125,7 +125,7 @@ //! while running. //! //!```no_run -//! # use franka::{Finishable, JointPositions}; +//! # use franka::{MotionFinished, JointPositions}; //! # fn joint_positions() -> JointPositions { //! # let time = 0.; //! # let mut out = JointPositions::new([0.;7]); @@ -153,9 +153,9 @@ //! ```no_run //! # use std::time::Duration; //! # use std::f64::consts::PI; -//! # use franka::{JointPositions, MotionFinished, RobotState, Robot, FrankaResult}; +//! # use franka::{JointPositions, FR3, RobotWrapper, 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) //! # } diff --git a/src/robot.rs b/src/robot.rs index 5549219..d0e9d0d 100644 --- a/src/robot.rs +++ b/src/robot.rs @@ -54,9 +54,9 @@ pub trait RobotWrapper { /// /// This minimal example will print the robot state 100 times: /// ```no_run - /// use franka::{Panda, RobotState, FrankaResult}; + /// use franka::{FR3, RobotState, RobotWrapper, FrankaResult}; /// fn main() -> FrankaResult<()> { - /// let mut robot = Panda::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); @@ -1348,7 +1348,7 @@ mod tests { }; use crate::robot::types::PandaStateIntern; use crate::robot::{RobotWrapper, FR3}; - use crate::{Finishable, FrankaResult, JointPositions, Panda, RealtimeConfig, RobotState}; + use crate::{FrankaResult, JointPositions, MotionFinished, Panda, RealtimeConfig, RobotState}; use bincode::{deserialize, serialize, serialized_size}; use std::iter::FromIterator; use std::mem::size_of; diff --git a/src/robot/control_loop.rs b/src/robot/control_loop.rs index f4788ca..58f21e9 100644 --- a/src/robot/control_loop.rs +++ b/src/robot/control_loop.rs @@ -216,8 +216,7 @@ impl< } command.tau_J_d = control_output.tau_J; command.tau_J_d.iter().for_each(|x| assert!(x.is_finite())); - // !control_output.is_finished() - true + !control_output.is_finished() } fn spin_motion( &mut self, diff --git a/src/robot/control_types.rs b/src/robot/control_types.rs index 86c31bf..85999ef 100644 --- a/src/robot/control_types.rs +++ b/src/robot/control_types.rs @@ -38,9 +38,6 @@ pub enum RealtimeConfig { Ignore, } -/// Helper type for control and motion generation loops. -/// -/// Used to determine whether to terminate a loop after the control callback has returned. pub trait ConvertMotion { /// converts the motion type to a MotionGeneratorCommand and applies rate limiting and filtering fn convert_motion( @@ -52,53 +49,28 @@ pub trait ConvertMotion { ); } +/// Helper type for control and motion generation loops. +/// +/// Used to determine whether to terminate a loop after the control callback has returned. pub trait Finishable { /// Determines whether to finish a currently running motion. 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); +} + +/// A trait for a Finshable control type to finish the motion +pub trait MotionFinished { + /// Helper method to indicate that a motion should stop after processing the given command. fn motion_finished(self) -> Self; } -// /// A trait for a Finshable control type to finish the motion -// pub trait MotionFinished : Finishable { -// /// Helper method to indicate that a motion should stop after processing the given command. -// fn motion_finished(self) -> Self; -// } -// -// // impl + Copy> MotionFinished for T { -// // type State4 = PandaState ; -// // -// // fn motion_finished(mut self) -> Self { -// // self.set_motion_finished(true); -// // self -// // } -// // } -// -// impl + Copy> MotionFinished for T { -// -// fn motion_finished(mut self) -> Self { -// self.set_motion_finished(true); -// self -// } -// } -// -// impl + Copy> MotionFinished for T { -// -// fn motion_finished(mut self) -> Self { -// self.set_motion_finished(true); -// self -// } -// } - -// impl + Copy,X> MotionFinished for T { -// type State4 = FR3State ; -// -// fn motion_finished(mut self) -> Self { -// self.set_motion_finished(true); -// self -// } -// } +impl MotionFinished for T { + fn motion_finished(mut self) -> Self { + self.set_motion_finished(true); + self + } +} /// Stores joint-level torque commands without gravity and friction. #[derive(Serialize, Deserialize, Debug, Copy, Clone)] @@ -134,46 +106,8 @@ impl Finishable for Torques { fn set_motion_finished(&mut self, finished: bool) { self.motion_finished = finished; } - fn motion_finished(mut self) -> Self { - self.set_motion_finished(true); - self - } } -impl ConvertMotion for Torques { - #[allow(unused_variables)] - //todo pull convert motion out of the Finishable trait - fn convert_motion( - &self, - robot_state: &Statee, - command: &mut MotionGeneratorCommand, - cutoff_frequency: f64, - limit_rate: bool, - ) { - unimplemented!() - } -} -// impl Finishable for Torques { -// -// fn is_finished(&self) -> bool { -// self.motion_finished -// } -// 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: &Self::State3, -// command: &mut MotionGeneratorCommand, -// cutoff_frequency: f64, -// limit_rate: bool, -// ) { -// unimplemented!() -// } -// } - /// Stores values for joint position motion generation. #[derive(Serialize, Deserialize, Debug, Copy, Clone)] #[allow(non_snake_case)] @@ -207,10 +141,6 @@ impl Finishable for JointPositions { fn set_motion_finished(&mut self, finished: bool) { self.motion_finished = finished; } - fn motion_finished(mut self) -> Self { - self.set_motion_finished(true); - self - } } impl ConvertMotion for JointPositions { fn convert_motion( @@ -245,39 +175,6 @@ impl ConvertMotion for JointPositions { command.q_c.iter().for_each(|x| assert!(x.is_finite())); } } -// impl ConvertMotion for JointPositions { -// fn convert_motion( -// &self, -// robot_state: &FR3State, -// command: &mut MotionGeneratorCommand, -// cutoff_frequency: f64, -// limit_rate: bool, -// ) { -// command.q_c = self.q; -// if cutoff_frequency < MAX_CUTOFF_FREQUENCY { -// for i in 0..7 { -// command.q_c[i] = low_pass_filter( -// DELTA_T, -// command.q_c[i], -// robot_state.q_d[i], -// cutoff_frequency, -// ); -// } -// } -// if limit_rate { -// command.q_c = limit_rate_joint_positions( -// &MAX_JOINT_VELOCITY, -// &MAX_JOINT_ACCELERATION, -// &MAX_JOINT_JERK, -// &command.q_c, -// &robot_state.q_d, -// &robot_state.dq_d, -// &robot_state.ddq_d, -// ); -// } -// command.q_c.iter().for_each(|x| assert!(x.is_finite())); -// } -// } impl MotionGeneratorTrait for JointPositions { fn get_motion_generator_mode() -> MoveMotionGeneratorMode { @@ -318,10 +215,6 @@ impl Finishable for JointVelocities { fn set_motion_finished(&mut self, finished: bool) { self.motion_finished = finished; } - fn motion_finished(mut self) -> Self { - self.set_motion_finished(true); - self - } } impl ConvertMotion for JointVelocities { fn convert_motion( @@ -356,39 +249,6 @@ impl ConvertMotion for JointVelocities { } } -// impl ConvertMotion for JointVelocities { -// fn convert_motion( -// &self, -// robot_state: &FR3State, -// command: &mut MotionGeneratorCommand, -// cutoff_frequency: f64, -// limit_rate: bool, -// ) { -// command.dq_c = self.dq; -// if cutoff_frequency < MAX_CUTOFF_FREQUENCY { -// for i in 0..7 { -// command.dq_c[i] = low_pass_filter( -// DELTA_T, -// command.dq_c[i], -// robot_state.dq_d[i], -// cutoff_frequency, -// ); -// } -// } -// if limit_rate { -// command.dq_c = limit_rate_joint_velocities( -// &MAX_JOINT_VELOCITY, -// &MAX_JOINT_ACCELERATION, -// &MAX_JOINT_JERK, -// &command.dq_c, -// &robot_state.dq_d, -// &robot_state.ddq_d, -// ); -// } -// command.dq_c.iter().for_each(|x| assert!(x.is_finite())); -// } -// } - impl MotionGeneratorTrait for JointVelocities { fn get_motion_generator_mode() -> MoveMotionGeneratorMode { MoveMotionGeneratorMode::JointVelocity @@ -469,10 +329,6 @@ impl Finishable for CartesianPose { fn set_motion_finished(&mut self, finished: bool) { self.motion_finished = finished; } - fn motion_finished(mut self) -> Self { - self.set_motion_finished(true); - self - } } impl ConvertMotion for CartesianPose { @@ -539,71 +395,6 @@ impl ConvertMotion for CartesianPose { } } -// impl ConvertMotion for CartesianPose { -// -// fn convert_motion( -// &self, -// robot_state: &FR3State, -// command: &mut MotionGeneratorCommand, -// cutoff_frequency: f64, -// limit_rate: bool, -// ) { -// command.O_T_EE_c = self.O_T_EE; -// if cutoff_frequency < MAX_CUTOFF_FREQUENCY { -// command.O_T_EE_c = cartesian_low_pass_filter( -// DELTA_T, -// &command.O_T_EE_c, -// &robot_state.O_T_EE_c, -// cutoff_frequency, -// ); -// } -// -// 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, -// &command.O_T_EE_c, -// &robot_state.O_T_EE_c, -// &robot_state.O_dP_EE_c, -// &robot_state.O_ddP_EE_c, -// ); -// } -// check_matrix(&command.O_T_EE_c); -// -// if self.has_elbow() { -// command.valid_elbow = true; -// command.elbow_c = self.elbow.unwrap(); -// if cutoff_frequency < MAX_CUTOFF_FREQUENCY { -// command.elbow_c[0] = low_pass_filter( -// DELTA_T, -// command.elbow_c[0], -// robot_state.elbow_c[0], -// cutoff_frequency, -// ); -// } -// if limit_rate { -// command.elbow_c[0] = limit_rate_position( -// MAX_ELBOW_VELOCITY, -// MAX_ELBOW_ACCELERATION, -// MAX_ELBOW_JERK, -// command.elbow_c[0], -// robot_state.elbow_c[0], -// robot_state.delbow_c[0], -// robot_state.ddelbow_c[0], -// ); -// } -// CartesianPose::check_elbow(&command.elbow_c); -// } else { -// command.valid_elbow = false; -// command.elbow_c = [0.; 2]; -// } -// } -// } - impl MotionGeneratorTrait for CartesianPose { fn get_motion_generator_mode() -> MoveMotionGeneratorMode { MoveMotionGeneratorMode::CartesianPosition @@ -660,10 +451,6 @@ impl Finishable for CartesianVelocities { fn set_motion_finished(&mut self, finished: bool) { self.motion_finished = finished; } - fn motion_finished(mut self) -> Self { - self.set_motion_finished(true); - self - } } impl ConvertMotion for CartesianVelocities { fn convert_motion( @@ -732,74 +519,6 @@ impl ConvertMotion for CartesianVelocities { } } -// impl ConvertMotion for CartesianVelocities { -// -// fn convert_motion( -// &self, -// robot_state: &FR3State, -// command: &mut MotionGeneratorCommand, -// cutoff_frequency: f64, -// limit_rate: bool, -// ) { -// command.O_dP_EE_c = self.O_dP_EE; -// if cutoff_frequency < MAX_CUTOFF_FREQUENCY { -// for i in 0..6 { -// command.O_dP_EE_c[i] = low_pass_filter( -// DELTA_T, -// command.O_dP_EE_c[i], -// robot_state.O_dP_EE_c[i], -// cutoff_frequency, -// ); -// } -// } -// 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, -// &command.O_dP_EE_c, -// &robot_state.O_dP_EE_c, -// &robot_state.O_ddP_EE_c, -// ); -// } -// command -// .O_dP_EE_c -// .iter() -// .for_each(|x| assert!(x.is_finite())); -// -// if self.has_elbow() { -// command.valid_elbow = true; -// command.elbow_c = self.elbow.unwrap(); -// if cutoff_frequency < MAX_CUTOFF_FREQUENCY { -// command.elbow_c[0] = low_pass_filter( -// DELTA_T, -// command.elbow_c[0], -// robot_state.elbow_c[0], -// cutoff_frequency, -// ); -// } -// if limit_rate { -// command.elbow_c[0] = limit_rate_position( -// MAX_ELBOW_VELOCITY, -// MAX_ELBOW_ACCELERATION, -// MAX_ELBOW_JERK, -// command.elbow_c[0], -// robot_state.elbow_c[0], -// robot_state.delbow_c[0], -// robot_state.ddelbow_c[0], -// ); -// } -// CartesianPose::check_elbow(&command.elbow_c); -// } else { -// command.valid_elbow = false; -// command.elbow_c = [0.; 2]; -// } -// } -// } - impl MotionGeneratorTrait for CartesianVelocities { fn get_motion_generator_mode() -> MoveMotionGeneratorMode { MoveMotionGeneratorMode::CartesianVelocity From e9fe50a9e221213b5bc037e391c0f98b2640ac53 Mon Sep 17 00:00:00 2001 From: Marco Boneberger Date: Fri, 21 Apr 2023 14:51:56 +0200 Subject: [PATCH 22/63] fix ordering for implementations of RobotData --- src/network.rs | 186 ++++++++++++++++++++++++------------------------- 1 file changed, 93 insertions(+), 93 deletions(-) diff --git a/src/network.rs b/src/network.rs index 86831f3..3de9a98 100644 --- a/src/network.rs +++ b/src/network.rs @@ -230,21 +230,46 @@ impl DeviceData for PandaData { } impl RobotData for PandaData { - type State = RobotState; type DeviceData = Self; + type Header = PandaCommandHeader; + type State = RobotState; type StateIntern = PandaStateIntern; type Model = PandaModel; - 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, @@ -261,8 +286,6 @@ impl RobotData for PandaData { ) } - type MoveStatus = MoveStatusPanda; - fn handle_command_move_status(status: Self::MoveStatus) -> Result<(), FrankaException> { match status { MoveStatusPanda::Success => Ok(()), @@ -331,7 +354,6 @@ impl RobotData for PandaData { log: Some(log), } } - fn create_control_exception_if_reflex_aborted( message: String, move_status: Self::MoveStatus, @@ -350,8 +372,6 @@ impl RobotData for PandaData { Ok(()) } - type GetterSetterStatus = GetterSetterStatusPanda; - fn handle_getter_setter_status(status: Self::GetterSetterStatus) -> FrankaResult<()> { match status { GetterSetterStatusPanda::Success => Ok(()), @@ -364,46 +384,6 @@ impl RobotData for PandaData { } } - 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!", - )), - } - } - - type StopMoveStatus = StopMoveStatusPanda; - type ConnectRequestWithHeader = ConnectRequestWithPandaHeader; - - 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, - } - } - - type AutomaticErrorRecoveryStatus = AutomaticErrorRecoveryStatusPanda; - fn handle_automatic_error_recovery_status( status: Self::AutomaticErrorRecoveryStatus, ) -> FrankaResult<()> { @@ -430,6 +410,26 @@ impl RobotData for PandaData { } } } + + 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!", + )), + } + } } impl DeviceData for FR3Data { type CommandHeader = FR3CommandHeader; @@ -460,12 +460,35 @@ impl RobotData for FR3Data { 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, @@ -524,28 +547,6 @@ impl RobotData for FR3Data { } } - 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.", - )) - } - } fn create_control_exception( message: String, @@ -581,7 +582,6 @@ impl RobotData for FR3Data { log: Some(log), } } - fn create_control_exception_if_reflex_aborted( message: String, move_status: Self::MoveStatus, @@ -614,29 +614,6 @@ impl RobotData for FR3Data { } } - type StopMoveStatus = StopMoveStatusFR3; - type ConnectRequestWithHeader = ConnectRequestWithFR3Header; - - 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, - } - } - - type AutomaticErrorRecoveryStatus = AutomaticErrorRecoveryStatusFR3; - fn handle_automatic_error_recovery_status( status: Self::AutomaticErrorRecoveryStatus, ) -> FrankaResult<()> { @@ -666,6 +643,29 @@ impl RobotData for FR3Data { )), } } + + 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.", + )) + } + } } impl DeviceData for GripperData { From 034daf9a691aa80255e3a80fce61313db14ccda2 Mon Sep 17 00:00:00 2001 From: Marco Boneberger Date: Fri, 21 Apr 2023 15:04:22 +0200 Subject: [PATCH 23/63] delete commented out code and fix implementation order --- src/network.rs | 1 - src/robot.rs | 332 ++++++++++++++++++------------------- src/robot/service_types.rs | 61 ------- 3 files changed, 166 insertions(+), 228 deletions(-) diff --git a/src/network.rs b/src/network.rs index 3de9a98..e1187c7 100644 --- a/src/network.rs +++ b/src/network.rs @@ -103,7 +103,6 @@ pub trait RobotData: DeviceData { 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? - // type LoadModelResponse: DeserializeOwned + Copy + Clone + 'static; // todo is this static fine here? fn create_model_library_request( command_id: &mut u32, diff --git a/src/robot.rs b/src/robot.rs index d0e9d0d..645b0c7 100644 --- a/src/robot.rs +++ b/src/robot.rs @@ -639,6 +639,164 @@ where ) } + 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 read_once(&mut self) -> FrankaResult { + match ::get_rob_mut(self).read_once() { + Ok(state) => Ok(state.into()), + Err(e) => Err(e), + } + } + + #[allow(clippy::too_many_arguments)] + fn set_collision_behavior( + &mut self, + 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 command = ::Data::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); + ::Data::handle_getter_setter_status(status) + } + + 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], + )?; + self.set_joint_impedance([3000., 3000., 3000., 2500., 2500., 2000., 2000.])?; + self.set_cartesian_impedance([3000., 3000., 3000., 300., 300., 300.])?; + Ok(()) + } + + #[allow(non_snake_case)] + fn set_joint_impedance(&mut self, K_theta: [f64; 7]) -> FrankaResult<()> { + let command = ::Data::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); + ::Data::handle_getter_setter_status(status) + } + + #[allow(non_snake_case)] + fn set_cartesian_impedance(&mut self, K_x: [f64; 6]) -> FrankaResult<()> { + let command = ::Data::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); + ::Data::handle_getter_setter_status(status) + } + + #[allow(non_snake_case)] + fn set_load( + &mut self, + load_mass: f64, + F_x_Cload: [f64; 3], + load_inertia: [f64; 9], + ) -> FrankaResult<()> { + let command = ::Data::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); + ::Data::handle_getter_setter_status(status) + } + + fn set_guiding_mode(&mut self, guiding_mode: [bool; 6], elbow: bool) -> FrankaResult<()> { + let command = ::Data::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); + ::Data::handle_getter_setter_status(status) + } + + #[allow(non_snake_case)] + fn set_K(&mut self, EE_T_K: [f64; 16]) -> FrankaResult<()> { + let command = ::Data::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); + ::Data::handle_getter_setter_status(status) + } + + #[allow(non_snake_case)] + fn set_EE(&mut self, NE_T_EE: [f64; 16]) -> FrankaResult<()> { + let command = ::Data::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); + ::Data::handle_getter_setter_status(status) + } + + fn automatic_error_recovery(&mut self) -> FrankaResult<()> { + let command = ::Data::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); + ::Data::handle_automatic_error_recovery_status(status) + } + + fn stop(&mut self) -> FrankaResult<()> { + let command = ::Data::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 control_joint_velocities< F: FnMut(&RobotState, &Duration) -> JointVelocities, CM: Into>, @@ -847,164 +1005,6 @@ where ) } - 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 read_once(&mut self) -> FrankaResult { - match ::get_rob_mut(self).read_once() { - Ok(state) => Ok(state.into()), - Err(e) => Err(e), - } - } - - #[allow(clippy::too_many_arguments)] - fn set_collision_behavior( - &mut self, - 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 command = ::Data::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); - ::Data::handle_getter_setter_status(status) - } - - 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], - )?; - self.set_joint_impedance([3000., 3000., 3000., 2500., 2500., 2000., 2000.])?; - self.set_cartesian_impedance([3000., 3000., 3000., 300., 300., 300.])?; - Ok(()) - } - - #[allow(non_snake_case)] - fn set_joint_impedance(&mut self, K_theta: [f64; 7]) -> FrankaResult<()> { - let command = ::Data::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); - ::Data::handle_getter_setter_status(status) - } - - #[allow(non_snake_case)] - fn set_cartesian_impedance(&mut self, K_x: [f64; 6]) -> FrankaResult<()> { - let command = ::Data::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); - ::Data::handle_getter_setter_status(status) - } - - #[allow(non_snake_case)] - fn set_load( - &mut self, - load_mass: f64, - F_x_Cload: [f64; 3], - load_inertia: [f64; 9], - ) -> FrankaResult<()> { - let command = ::Data::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); - ::Data::handle_getter_setter_status(status) - } - - fn set_guiding_mode(&mut self, guiding_mode: [bool; 6], elbow: bool) -> FrankaResult<()> { - let command = ::Data::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); - ::Data::handle_getter_setter_status(status) - } - - #[allow(non_snake_case)] - fn set_K(&mut self, EE_T_K: [f64; 16]) -> FrankaResult<()> { - let command = ::Data::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); - ::Data::handle_getter_setter_status(status) - } - - #[allow(non_snake_case)] - fn set_EE(&mut self, NE_T_EE: [f64; 16]) -> FrankaResult<()> { - let command = ::Data::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); - ::Data::handle_getter_setter_status(status) - } - - fn automatic_error_recovery(&mut self) -> FrankaResult<()> { - let command = ::Data::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); - ::Data::handle_automatic_error_recovery_status(status) - } - - fn stop(&mut self) -> FrankaResult<()> { - let command = ::Data::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 load_model(&mut self, persistent: bool) -> FrankaResult { ::get_rob_mut(self).load_model(persistent) } @@ -1156,32 +1156,32 @@ impl Robot for Panda { } impl PrivateRobot for Panda { - fn get_net(&mut self) -> &mut Network { - &mut self.robimpl.network - } fn get_rob_mut(&mut self) -> &mut Self::Rob { &mut self.robimpl } - fn get_rob(&self) -> &Self::Rob { &self.robimpl } + + fn get_net(&mut self) -> &mut Network { + &mut self.robimpl.network + } } pub struct FR3 { robimpl: RobotImplGeneric, } impl PrivateRobot for FR3 { - fn get_net(&mut self) -> &mut Network { - &mut self.robimpl.network - } fn get_rob_mut(&mut self) -> &mut Self::Rob { &mut self.robimpl } - fn get_rob(&self) -> &Self::Rob { &self.robimpl } + + fn get_net(&mut self) -> &mut Network { + &mut self.robimpl.network + } } impl Robot for FR3 { type Data = FR3Data; diff --git a/src/robot/service_types.rs b/src/robot/service_types.rs index cf489d6..5ed5dfc 100644 --- a/src/robot/service_types.rs +++ b/src/robot/service_types.rs @@ -366,12 +366,6 @@ impl MessageCommand for StopMoveRequestWithPandaHeader { } } -// #[derive(Serialize, Deserialize, Debug)] -// pub struct StopMoveResponse { -// pub header: PandaCommandHeader, -// pub status: StopMoveStatus, -// } - #[derive(Serialize, Deserialize, Debug, Copy, Clone)] #[repr(packed)] pub struct GetCartesianLimitRequest { @@ -679,27 +673,6 @@ pub struct LoadModelLibraryRequest { pub system: LoadModelLibrarySystem, } -// #[derive(Serialize, Deserialize, Debug, Copy, Clone)] -// #[repr(packed)] -// pub struct LoadModelLibraryRequestWithHeader { -// pub header: PandaCommandHeader, -// pub request: LoadModelLibraryRequest, -// } - -// // this is a very hacky generic struct that is a generic version of LoadModelLibraryRequestWithHeader -// // todo find a better solution to deal with the model downloader -// #[derive(Serialize, Deserialize, Debug, Copy, Clone)] -// pub struct ModelRequestWithHeader { -// pub header: Header, -// pub request: LoadModelLibraryRequest, -// } -// -// impl MessageCommand for ModelRequestWithHeader
{ -// fn get_command_message_id(&self) -> u32 { -// self.header.get_command_message_id() -// } -// } - define_panda_request_with_header!( LoadModelLibraryRequestWithPandaHeader, LoadModelLibraryRequest, @@ -710,40 +683,6 @@ define_fr3_request_with_header!( LoadModelLibraryRequest, FR3CommandEnum::LoadModelLibrary ); -// -// impl MessageCommand for LoadModelLibraryRequestWithHeader { -// fn get_command_message_id(&self) -> u32 { -// self.header.get_command_message_id() -// } -// } - -// impl From<(u32, LoadModelLibraryRequest)> for LoadModelLibraryRequestWithPandaHeader { -// fn from(tuple: (u32, LoadModelLibraryRequest)) -> Self { -// let command_id = tuple.0; -// LoadModelLibraryRequestWithPandaHeader { -// header: PandaCommandHeader::new( -// PandaCommandEnum::LoadModelLibrary, -// command_id, -// std::mem::size_of::() as u32, -// ), -// request: tuple.1, -// } -// } -// } - -// impl From<(u32, LoadModelLibraryRequest)> for LoadModelLibraryRequestWithFR3Header { -// fn from(tuple: (u32, LoadModelLibraryRequest)) -> Self { -// let command_id = tuple.0; -// LoadModelLibraryRequestWithFR3Header { -// header: FR3CommandHeader::new( -// FR3CommandEnum::LoadModelLibrary, -// command_id, -// std::mem::size_of::() as u32, -// ), -// request: tuple.1, -// } -// } -// } #[derive(Serialize, Deserialize, Debug, Copy, Clone)] #[repr(packed)] From f85f7e82d06115d98b96e0637d5913925a9a8132 Mon Sep 17 00:00:00 2001 From: Marco Boneberger Date: Fri, 21 Apr 2023 15:22:44 +0200 Subject: [PATCH 24/63] update badges --- README.md | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/README.md b/README.md index 0dcd04f..c140ee8 100644 --- a/README.md +++ b/README.md @@ -1,8 +1,8 @@ -[![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) # 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. From 8d4df67b1ed9c94ad7995255e09be6a5afa3680b Mon Sep 17 00:00:00 2001 From: Marco Boneberger Date: Fri, 21 Apr 2023 15:29:26 +0200 Subject: [PATCH 25/63] rename FR3 to Fr3 --- examples/cartesian_impedance_control.rs | 6 +- examples/communication_test.rs | 4 +- examples/download_model.rs | 4 +- examples/echo_robot_state.rs | 4 +- examples/generate_cartesian_pose_motion.rs | 4 +- .../generate_cartesian_velocity_motion.rs | 4 +- examples/generate_consecutive_motions.rs | 4 +- examples/generate_elbow_motion.rs | 4 +- examples/generate_joint_position_motion.rs | 4 +- examples/generate_joint_velocity_motion.rs | 4 +- examples/mirror_robot.rs | 8 +- examples/print_joint_poses.rs | 4 +- src/lib.rs | 22 +-- src/model.rs | 6 +- src/network.rs | 158 +++++++++--------- src/robot.rs | 56 +++---- src/robot/service_types.rs | 76 ++++----- src/robot/types.rs | 2 +- 18 files changed, 187 insertions(+), 187 deletions(-) diff --git a/examples/cartesian_impedance_control.rs b/examples/cartesian_impedance_control.rs index 8c65dcf..7ecf272 100644 --- a/examples/cartesian_impedance_control.rs +++ b/examples/cartesian_impedance_control.rs @@ -7,8 +7,8 @@ use clap::Parser; use nalgebra::{Matrix3, Matrix6, Matrix6x1, UnitQuaternion, Vector3, U1, U3}; use franka::{ - array_to_isometry, Frame, FrankaResult, Matrix6x7, Panda, RobotModel, RobotState, RobotWrapper, - Torques, Vector7, FR3, + array_to_isometry, Fr3, Frame, FrankaResult, Matrix6x7, Panda, RobotModel, RobotState, + RobotWrapper, Torques, Vector7, }; /// An example showing a simple cartesian impedance controller without inertia shaping @@ -35,7 +35,7 @@ fn main() -> FrankaResult<()> { generate_motion(robot) } false => { - let robot = FR3::new(args.franka_ip.as_str(), None, None)?; + let robot = Fr3::new(args.franka_ip.as_str(), None, None)?; generate_motion(robot) } } diff --git a/examples/communication_test.rs b/examples/communication_test.rs index 2c3dac4..cd76f6c 100644 --- a/examples/communication_test.rs +++ b/examples/communication_test.rs @@ -5,7 +5,7 @@ use std::time::Duration; use clap::Parser; -use franka::{FrankaResult, MotionFinished, Panda, RobotState, RobotWrapper, Torques, FR3}; +use franka::{Fr3, FrankaResult, MotionFinished, Panda, RobotState, RobotWrapper, Torques}; /// An example indicating the network performance. /// @@ -28,7 +28,7 @@ fn main() -> FrankaResult<()> { generate_motion(robot) } false => { - let robot = FR3::new(address.franka_ip.as_str(), None, None)?; + let robot = Fr3::new(address.franka_ip.as_str(), None, None)?; generate_motion(robot) } } diff --git a/examples/download_model.rs b/examples/download_model.rs index 1011eeb..bc5f87d 100644 --- a/examples/download_model.rs +++ b/examples/download_model.rs @@ -7,7 +7,7 @@ use std::path::PathBuf; use clap::Parser; use franka::exception::FrankaException::ModelException; -use franka::{FrankaResult, Panda, RealtimeConfig, RobotWrapper, FR3}; +use franka::{Fr3, FrankaResult, Panda, RealtimeConfig, RobotWrapper}; /// Downloads the model for offline usage #[derive(Parser, Debug)] @@ -34,7 +34,7 @@ fn main() -> FrankaResult<()> { download_model(robot, path) } false => { - let robot = FR3::new(args.franka_ip.as_str(), RealtimeConfig::Ignore, None)?; + let robot = Fr3::new(args.franka_ip.as_str(), RealtimeConfig::Ignore, None)?; download_model(robot, path) } } diff --git a/examples/echo_robot_state.rs b/examples/echo_robot_state.rs index 4acff88..4f56310 100644 --- a/examples/echo_robot_state.rs +++ b/examples/echo_robot_state.rs @@ -3,7 +3,7 @@ use clap::Parser; -use franka::{FrankaResult, Panda, RobotState, RobotWrapper, FR3}; +use franka::{Fr3, FrankaResult, Panda, RobotState, RobotWrapper}; /// An example showing how to continuously read the robot state. #[derive(Parser, Debug)] @@ -24,7 +24,7 @@ fn main() -> FrankaResult<()> { echo_robot_state(robot) } false => { - let robot = FR3::new(args.franka_ip.as_str(), None, None)?; + let robot = Fr3::new(args.franka_ip.as_str(), None, None)?; echo_robot_state(robot) } } diff --git a/examples/generate_cartesian_pose_motion.rs b/examples/generate_cartesian_pose_motion.rs index 27db22d..22dfcca 100644 --- a/examples/generate_cartesian_pose_motion.rs +++ b/examples/generate_cartesian_pose_motion.rs @@ -6,7 +6,7 @@ use std::time::Duration; use clap::Parser; -use franka::{CartesianPose, FrankaResult, MotionFinished, Panda, RobotState, RobotWrapper, FR3}; +use franka::{CartesianPose, Fr3, FrankaResult, MotionFinished, Panda, RobotState, RobotWrapper}; /// An example showing how to generate a Cartesian motion. /// @@ -30,7 +30,7 @@ fn main() -> FrankaResult<()> { generate_motion(robot) } false => { - let robot = FR3::new(args.franka_ip.as_str(), None, None)?; + let robot = Fr3::new(args.franka_ip.as_str(), None, None)?; generate_motion(robot) } } diff --git a/examples/generate_cartesian_velocity_motion.rs b/examples/generate_cartesian_velocity_motion.rs index 3dcb36e..ab51d0f 100644 --- a/examples/generate_cartesian_velocity_motion.rs +++ b/examples/generate_cartesian_velocity_motion.rs @@ -7,7 +7,7 @@ use std::time::Duration; use clap::Parser; use franka::{ - CartesianVelocities, FrankaResult, MotionFinished, Panda, RobotState, RobotWrapper, FR3, + CartesianVelocities, Fr3, FrankaResult, MotionFinished, Panda, RobotState, RobotWrapper, }; /// An example showing how to generate a Cartesian velocity motion. @@ -31,7 +31,7 @@ fn main() -> FrankaResult<()> { generate_motion(robot) } false => { - let robot = FR3::new(args.franka_ip.as_str(), None, None)?; + let robot = Fr3::new(args.franka_ip.as_str(), None, None)?; generate_motion(robot) } } diff --git a/examples/generate_consecutive_motions.rs b/examples/generate_consecutive_motions.rs index 0a7c220..58ee01a 100644 --- a/examples/generate_consecutive_motions.rs +++ b/examples/generate_consecutive_motions.rs @@ -7,7 +7,7 @@ use std::time::Duration; use clap::Parser; use franka::exception::FrankaException; -use franka::{FrankaResult, JointVelocities, MotionFinished, Panda, RobotState, RobotWrapper, FR3}; +use franka::{Fr3, FrankaResult, JointVelocities, MotionFinished, Panda, RobotState, RobotWrapper}; /// An example showing how to execute consecutive motions with error recovery. /// @@ -31,7 +31,7 @@ fn main() -> FrankaResult<()> { generate_motion(robot) } false => { - let robot = FR3::new(args.franka_ip.as_str(), None, None)?; + let robot = Fr3::new(args.franka_ip.as_str(), None, None)?; generate_motion(robot) } } diff --git a/examples/generate_elbow_motion.rs b/examples/generate_elbow_motion.rs index c3be957..a7b8164 100644 --- a/examples/generate_elbow_motion.rs +++ b/examples/generate_elbow_motion.rs @@ -6,7 +6,7 @@ use std::time::Duration; use clap::Parser; -use franka::{CartesianPose, FrankaResult, MotionFinished, Panda, RobotState, RobotWrapper, FR3}; +use franka::{CartesianPose, Fr3, FrankaResult, MotionFinished, Panda, RobotState, RobotWrapper}; /// An example showing how to move the robot's elbow. /// @@ -29,7 +29,7 @@ fn main() -> FrankaResult<()> { generate_motion(robot) } false => { - let robot = FR3::new(args.franka_ip.as_str(), None, None)?; + let robot = Fr3::new(args.franka_ip.as_str(), None, None)?; generate_motion(robot) } } diff --git a/examples/generate_joint_position_motion.rs b/examples/generate_joint_position_motion.rs index e2f0f5d..8287273 100644 --- a/examples/generate_joint_position_motion.rs +++ b/examples/generate_joint_position_motion.rs @@ -6,7 +6,7 @@ use std::time::Duration; use clap::Parser; -use franka::{FrankaResult, JointPositions, MotionFinished, Panda, RobotState, RobotWrapper, FR3}; +use franka::{Fr3, FrankaResult, JointPositions, MotionFinished, Panda, RobotState, RobotWrapper}; /// An example showing how to generate a joint position motion. /// @@ -29,7 +29,7 @@ fn main() -> FrankaResult<()> { generate_motion(robot) } false => { - let robot = FR3::new(args.franka_ip.as_str(), None, None)?; + let robot = Fr3::new(args.franka_ip.as_str(), None, None)?; generate_motion(robot) } } diff --git a/examples/generate_joint_velocity_motion.rs b/examples/generate_joint_velocity_motion.rs index 8b304af..c473868 100644 --- a/examples/generate_joint_velocity_motion.rs +++ b/examples/generate_joint_velocity_motion.rs @@ -6,7 +6,7 @@ use std::time::Duration; use clap::Parser; -use franka::{FrankaResult, JointVelocities, MotionFinished, Panda, RobotState, RobotWrapper, FR3}; +use franka::{Fr3, FrankaResult, JointVelocities, MotionFinished, Panda, RobotState, RobotWrapper}; /// An example showing how to generate a joint velocity motion. /// @@ -29,7 +29,7 @@ fn main() -> FrankaResult<()> { generate_motion(robot) } false => { - let robot = FR3::new(args.franka_ip.as_str(), None, None)?; + let robot = Fr3::new(args.franka_ip.as_str(), None, None)?; generate_motion(robot) } } diff --git a/examples/mirror_robot.rs b/examples/mirror_robot.rs index a626d29..4599ec6 100644 --- a/examples/mirror_robot.rs +++ b/examples/mirror_robot.rs @@ -9,8 +9,8 @@ use clap::Parser; use nalgebra::{Matrix3, Matrix6, Matrix6x1, Quaternion, UnitQuaternion, Vector3, U1, U3}; use franka::{ - array_to_isometry, Frame, FrankaResult, Matrix6x7, Matrix7, Panda, RobotModel, RobotState, - RobotWrapper, Torques, Vector7, FR3, + array_to_isometry, Fr3, Frame, FrankaResult, Matrix6x7, Matrix7, Panda, RobotModel, RobotState, + RobotWrapper, Torques, Vector7, }; /// An example where one robot is guided by the user and the other robot acts as a mirror. In this @@ -46,7 +46,7 @@ fn main() -> FrankaResult<()> { initialize_and_start_robots(&args, robot_user) } false => { - let robot_user = FR3::new(args.franka_ip_user.as_str(), None, None)?; + let robot_user = Fr3::new(args.franka_ip_user.as_str(), None, None)?; initialize_and_start_robots(&args, robot_user) } } @@ -62,7 +62,7 @@ fn initialize_and_start_robots( control_robots(robot_user, robot_mirror) } false => { - let robot_mirror = FR3::new(args.franka_ip_mirror.as_str(), None, None)?; + let robot_mirror = Fr3::new(args.franka_ip_mirror.as_str(), None, None)?; control_robots(robot_user, robot_mirror) } } diff --git a/examples/print_joint_poses.rs b/examples/print_joint_poses.rs index a6704d1..b86a07e 100644 --- a/examples/print_joint_poses.rs +++ b/examples/print_joint_poses.rs @@ -3,7 +3,7 @@ use clap::Parser; use nalgebra::Matrix4; -use franka::{Frame, FrankaResult, Panda, RobotModel, RobotWrapper, FR3}; +use franka::{Fr3, Frame, FrankaResult, Panda, RobotModel, RobotWrapper}; /// An example showing how to use the model library that prints the transformation /// matrix of each joint with respect to the base frame. @@ -25,7 +25,7 @@ fn main() -> FrankaResult<()> { generate_motion(robot) } false => { - let robot = FR3::new(args.franka_ip.as_str(), None, None)?; + let robot = Fr3::new(args.franka_ip.as_str(), None, None)?; generate_motion(robot) } } diff --git a/src/lib.rs b/src/lib.rs index 1393fba..75d8bec 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, RobotWrapper, RobotState, FR3, FrankaResult}; +//! use franka::{JointPositions, MotionFinished, RobotWrapper, RobotState, Fr3, FrankaResult}; //! fn main() -> FrankaResult<()> { -//! let mut robot = FR3::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], @@ -60,9 +60,9 @@ //!```no_run //! # use std::time::Duration; //! # use std::f64::consts::PI; -//! # use franka::{JointPositions, MotionFinished, RobotState, FR3, FrankaResult}; +//! # use franka::{JointPositions, MotionFinished, RobotState, Fr3, FrankaResult}; //! # fn main() -> FrankaResult<()> { -//! let mut robot = FR3::new("robotik-bs.de", None, None)?; +//! let mut robot = Fr3::new("robotik-bs.de", None, None)?; //! # Ok(()) //! # } //! ``` @@ -80,9 +80,9 @@ //!```no_run //! # use std::time::Duration; //! # use std::f64::consts::PI; -//! # use franka::{JointPositions, MotionFinished, RobotState, FR3, RobotWrapper, FrankaResult}; +//! # use franka::{JointPositions, MotionFinished, RobotState, Fr3, RobotWrapper, FrankaResult}; //! # fn main() -> FrankaResult<()> { -//! # let mut robot = FR3::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(()) @@ -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, FR3, RobotWrapper, RobotState, FrankaResult}; +//! # use franka::{JointPositions, Fr3, RobotWrapper, RobotState, FrankaResult}; //! # fn main() -> FrankaResult<()> { -//! # let mut robot = FR3::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) //! # } @@ -174,7 +174,7 @@ pub mod utils; pub use exception::FrankaResult; pub use gripper::gripper_state::GripperState; pub use gripper::Gripper; -pub use model::FR3Model; +pub use model::Fr3Model; pub use model::Frame; pub use model::PandaModel; pub use model::RobotModel; @@ -183,7 +183,7 @@ 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::RobotWrapper; -pub use robot::FR3; pub use utils::*; diff --git a/src/model.rs b/src/model.rs index d6b04d9..488abfc 100644 --- a/src/model.rs +++ b/src/model.rs @@ -461,12 +461,12 @@ impl RobotModel for PandaModel { } /// Calculates poses of joints and dynamic properties of the robot. -pub struct FR3Model { +pub struct Fr3Model { library: ModelLibrary, } #[allow(non_snake_case)] -impl RobotModel for FR3Model { +impl RobotModel for Fr3Model { /// 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 @@ -496,7 +496,7 @@ impl RobotModel for FR3Model { /// /// How does the libfranka embed libm? They are not including - Apparently it gets somehow included when using \ ¯\_(ツ)_/¯ fn new>(model_filename: S, libm_filename: Option<&Path>) -> FrankaResult { - Ok(FR3Model { + Ok(Fr3Model { library: ModelLibrary::new(model_filename.as_ref(), libm_filename)?, }) } diff --git a/src/network.rs b/src/network.rs index e1187c7..c7aa710 100644 --- a/src/network.rs +++ b/src/network.rs @@ -34,31 +34,31 @@ use crate::robot::errors::FrankaErrors; use crate::robot::logger::Record; use crate::robot::robot_state::AbstractRobotState; use crate::robot::service_types::{ - AutomaticErrorRecoveryStatusFR3, AutomaticErrorRecoveryStatusPanda, ConnectRequest, - ConnectRequestWithFR3Header, ConnectRequestWithPandaHeader, FR3CommandEnum, FR3CommandHeader, - GetterSetterStatusFR3, GetterSetterStatusPanda, LoadModelLibraryRequest, - LoadModelLibraryRequestWithFR3Header, LoadModelLibraryRequestWithPandaHeader, - LoadModelLibraryStatus, MoveRequest, MoveRequestWithFR3Header, MoveRequestWithPandaHeader, - MoveStatusFR3, MoveStatusPanda, PandaCommandEnum, PandaCommandHeader, RobotHeader, - SetCartesianImpedanceRequest, SetCartesianImpedanceRequestWithFR3Header, + AutomaticErrorRecoveryStatusFr3, AutomaticErrorRecoveryStatusPanda, ConnectRequest, + ConnectRequestWithFr3Header, ConnectRequestWithPandaHeader, Fr3CommandEnum, Fr3CommandHeader, + GetterSetterStatusFr3, GetterSetterStatusPanda, LoadModelLibraryRequest, + LoadModelLibraryRequestWithFr3Header, LoadModelLibraryRequestWithPandaHeader, + LoadModelLibraryStatus, MoveRequest, MoveRequestWithFr3Header, MoveRequestWithPandaHeader, + MoveStatusFr3, MoveStatusPanda, PandaCommandEnum, PandaCommandHeader, RobotHeader, + SetCartesianImpedanceRequest, SetCartesianImpedanceRequestWithFr3Header, SetCartesianImpedanceRequestWithPandaHeader, SetCollisionBehaviorRequest, - SetCollisionBehaviorRequestWithFR3Header, SetCollisionBehaviorRequestWithPandaHeader, - SetEeToKRequest, SetEeToKRequestWithFR3Header, SetEeToKRequestWithPandaHeader, - SetGuidingModeRequest, SetGuidingModeRequestWithFR3Header, + SetCollisionBehaviorRequestWithFr3Header, SetCollisionBehaviorRequestWithPandaHeader, + SetEeToKRequest, SetEeToKRequestWithFr3Header, SetEeToKRequestWithPandaHeader, + SetGuidingModeRequest, SetGuidingModeRequestWithFr3Header, SetGuidingModeRequestWithPandaHeader, SetJointImpedanceRequest, - SetJointImpedanceRequestWithFR3Header, SetJointImpedanceRequestWithPandaHeader, SetLoadRequest, - SetLoadRequestWithFR3Header, SetLoadRequestWithPandaHeader, SetNeToEeRequest, - SetNeToEeRequestWithFR3Header, SetNeToEeRequestWithPandaHeader, StopMoveStatusFR3, + SetJointImpedanceRequestWithFr3Header, SetJointImpedanceRequestWithPandaHeader, SetLoadRequest, + SetLoadRequestWithFr3Header, SetLoadRequestWithPandaHeader, SetNeToEeRequest, + SetNeToEeRequestWithFr3Header, SetNeToEeRequestWithPandaHeader, StopMoveStatusFr3, StopMoveStatusPanda, FR3_VERSION, PANDA_VERSION, }; -use crate::robot::types::{AbstractRobotStateIntern, FR3StateIntern, PandaStateIntern}; -use crate::{FR3Model, PandaModel, RobotModel, RobotState}; +use crate::robot::types::{AbstractRobotStateIntern, Fr3StateIntern, PandaStateIntern}; +use crate::{Fr3Model, PandaModel, RobotModel, RobotState}; const CLIENT: Token = Token(1); pub enum NetworkType { Panda, - FR3, + Fr3, Gripper, } @@ -206,7 +206,7 @@ pub trait RobotData: DeviceData { pub struct PandaData {} -pub struct FR3Data {} +pub struct Fr3Data {} pub struct GripperData {} @@ -430,15 +430,15 @@ impl RobotData for PandaData { } } } -impl DeviceData for FR3Data { - type CommandHeader = FR3CommandHeader; - type CommandEnum = FR3CommandEnum; +impl DeviceData for Fr3Data { + 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); + let header = Fr3CommandHeader::new(command, *command_id, size as u32); *command_id += 1; header } @@ -447,29 +447,29 @@ impl DeviceData for FR3Data { FR3_VERSION } } -impl RobotData for FR3Data { +impl RobotData for Fr3Data { type DeviceData = Self; - type Header = FR3CommandHeader; + type Header = Fr3CommandHeader; type State = RobotState; - type StateIntern = FR3StateIntern; - type Model = FR3Model; - 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; + type StateIntern = Fr3StateIntern; + type Model = Fr3Model; + 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, @@ -478,10 +478,10 @@ impl RobotData for FR3Data { version: FR3_VERSION, udp_port, }; - ConnectRequestWithFR3Header { + ConnectRequestWithFr3Header { header: Self::create_header( command_id, - FR3CommandEnum::Connect, + Fr3CommandEnum::Connect, size_of::(), ), request, @@ -491,54 +491,54 @@ impl RobotData for FR3Data { fn create_automatic_error_recovery_request(command_id: &mut u32) -> Self::CommandHeader { Self::create_header( command_id, - FR3CommandEnum::AutomaticErrorRecovery, - size_of::(), + Fr3CommandEnum::AutomaticErrorRecovery, + size_of::(), ) } fn create_stop_request(command_id: &mut u32) -> Self::CommandHeader { Self::create_header( command_id, - FR3CommandEnum::StopMove, - size_of::(), + Fr3CommandEnum::StopMove, + size_of::(), ) } fn handle_command_move_status(status: Self::MoveStatus) -> Result<(), FrankaException> { match status { - MoveStatusFR3::Success => Ok(()), - MoveStatusFR3::MotionStarted => { + MoveStatusFr3::Success => Ok(()), + MoveStatusFr3::MotionStarted => { //todo handle motion_generator_running == true Ok(()) } - MoveStatusFR3::EmergencyAborted => Err(create_command_exception( + MoveStatusFr3::EmergencyAborted => Err(create_command_exception( "libfranka-rs: Move command aborted: User Stop pressed!", )), - MoveStatusFR3::ReflexAborted => Err(create_command_exception( + MoveStatusFr3::ReflexAborted => Err(create_command_exception( "libfranka-rs: Move command aborted: motion aborted by reflex!", )), - MoveStatusFR3::InputErrorAborted => Err(create_command_exception( + MoveStatusFr3::InputErrorAborted => Err(create_command_exception( "libfranka-rs: Move command aborted: invalid input provided!", )), - MoveStatusFR3::CommandNotPossibleRejected => Err(create_command_exception( + MoveStatusFr3::CommandNotPossibleRejected => Err(create_command_exception( "libfranka-rs: Move command rejected: command not possible in the current mode!", )), - MoveStatusFR3::StartAtSingularPoseRejected => Err(create_command_exception( + MoveStatusFr3::StartAtSingularPoseRejected => Err(create_command_exception( "libfranka-rs: Move command rejected: cannot start at singular pose!", )), - MoveStatusFR3::InvalidArgumentRejected => Err(create_command_exception( + MoveStatusFr3::InvalidArgumentRejected => Err(create_command_exception( "libfranka-rs: Move command rejected: maximum path deviation out of range!", )), - MoveStatusFR3::Preempted => Err(create_command_exception( + MoveStatusFr3::Preempted => Err(create_command_exception( "libfranka-rs: Move command preempted!", )), - MoveStatusFR3::Aborted => Err(create_command_exception( + MoveStatusFr3::Aborted => Err(create_command_exception( "libfranka-rs: Move command aborted!", )), - MoveStatusFR3::PreemptedDueToActivatedSafetyFunctions =>Err(create_command_exception( + MoveStatusFr3::PreemptedDueToActivatedSafetyFunctions =>Err(create_command_exception( "libfranka-rs: Move command preempted due to activated safety function! Please disable all safety functions.", )), - MoveStatusFR3::CommandRejectedDueToActivatedSafetyFunctions => + MoveStatusFr3::CommandRejectedDueToActivatedSafetyFunctions => Err(create_command_exception( "libfranka-rs: Move command rejected due to activated safety function! Please disable all safety functions.", )) @@ -554,7 +554,7 @@ impl RobotData for FR3Data { log: Vec>, ) -> FrankaException { let mut exception_string = String::from(&message); - if move_status == MoveStatusFR3::ReflexAborted { + if move_status == MoveStatusFr3::ReflexAborted { exception_string += " "; exception_string += reflex_reasons.to_string().as_str(); if log.len() >= 2 { @@ -587,7 +587,7 @@ impl RobotData for FR3Data { reflex_reasons: &FrankaErrors, log: Vec>, ) -> FrankaResult<()> { - if move_status == MoveStatusFR3::ReflexAborted { + if move_status == MoveStatusFr3::ReflexAborted { return Err(Self::create_control_exception( message, move_status, @@ -600,14 +600,14 @@ impl RobotData for FR3Data { fn handle_getter_setter_status(status: Self::GetterSetterStatus) -> FrankaResult<()> { match status { - GetterSetterStatusFR3::Success => Ok(()), - GetterSetterStatusFR3::CommandNotPossibleRejected => Err(create_command_exception( + 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( + GetterSetterStatusFr3::InvalidArgumentRejected => Err(create_command_exception( "libfranka-rs: command rejected: invalid argument!", )), - GetterSetterStatusFR3::CommandRejectedDueToActivatedSafetyFunctions => Err(create_command_exception( + GetterSetterStatusFr3::CommandRejectedDueToActivatedSafetyFunctions => Err(create_command_exception( "libfranka-rs: command rejected due to activated safety function! Please disable all safety functions.", )), } @@ -617,27 +617,27 @@ impl RobotData for FR3Data { status: Self::AutomaticErrorRecoveryStatus, ) -> FrankaResult<()> { match &status { - AutomaticErrorRecoveryStatusFR3::Success => Ok(()), - AutomaticErrorRecoveryStatusFR3::EmergencyAborted => Err(create_command_exception( + AutomaticErrorRecoveryStatusFr3::Success => Ok(()), + AutomaticErrorRecoveryStatusFr3::EmergencyAborted => Err(create_command_exception( "libfranka-rs: command aborted: User Stop pressed!", )), - AutomaticErrorRecoveryStatusFR3::ReflexAborted => Err(create_command_exception( + AutomaticErrorRecoveryStatusFr3::ReflexAborted => Err(create_command_exception( "libfranka-rs: command aborted: motion aborted by reflex!", )), - AutomaticErrorRecoveryStatusFR3::CommandNotPossibleRejected => { + AutomaticErrorRecoveryStatusFr3::CommandNotPossibleRejected => { Err(create_command_exception( "libfranka-rs: command rejected: command not possible in current mode", )) } - AutomaticErrorRecoveryStatusFR3::ManualErrorRecoveryRequiredRejected => { + AutomaticErrorRecoveryStatusFr3::ManualErrorRecoveryRequiredRejected => { Err(create_command_exception( "libfranka-rs: command rejected: manual error recovery required!", )) } - AutomaticErrorRecoveryStatusFR3::Aborted => { + AutomaticErrorRecoveryStatusFr3::Aborted => { Err(create_command_exception("libfranka-rs: command aborted!")) } - AutomaticErrorRecoveryStatusFR3::CommandRejectedDueToActivatedSafetyFunctions => Err(create_command_exception( + AutomaticErrorRecoveryStatusFr3::CommandRejectedDueToActivatedSafetyFunctions => Err(create_command_exception( "libfranka-rs: command rejected due to activated safety function! Please disable all safety functions.", )), } @@ -647,20 +647,20 @@ impl RobotData for FR3Data { status: Self::StopMoveStatus, ) -> Result<(), FrankaException> { match status { - StopMoveStatusFR3::Success => Ok(()), - StopMoveStatusFR3::EmergencyAborted => Err(create_command_exception( + StopMoveStatusFr3::Success => Ok(()), + StopMoveStatusFr3::EmergencyAborted => Err(create_command_exception( "libfranka-rs: Stop command aborted: User Stop pressed!", )), - StopMoveStatusFR3::ReflexAborted => Err(create_command_exception( + StopMoveStatusFr3::ReflexAborted => Err(create_command_exception( "libfranka-rs: Stop command aborted: motion aborted by reflex!", )), - StopMoveStatusFR3::CommandNotPossibleRejected => Err(create_command_exception( + StopMoveStatusFr3::CommandNotPossibleRejected => Err(create_command_exception( "libfranka-rs: Stop command rejected: command not possible in the current mode!", )), - StopMoveStatusFR3::Aborted => Err(create_command_exception( + StopMoveStatusFr3::Aborted => Err(create_command_exception( "libfranka-rs: Stop command aborted!", )), - StopMoveStatusFR3::CommandRejectedDueToActivatedSafetyFunctions => Err(create_command_exception( + StopMoveStatusFr3::CommandRejectedDueToActivatedSafetyFunctions => Err(create_command_exception( "libfranka-rs: Move command rejected due to activated safety function! Please disable all safety functions.", )) } diff --git a/src/robot.rs b/src/robot.rs index 645b0c7..dc38824 100644 --- a/src/robot.rs +++ b/src/robot.rs @@ -8,7 +8,7 @@ use std::time::Duration; use std::fmt::Debug; use crate::exception::{create_command_exception, FrankaException, FrankaResult}; -use crate::network::{FR3Data, Network, PandaData, RobotData}; +use crate::network::{Fr3Data, Network, PandaData, RobotData}; use crate::robot::control_loop::ControlLoop; use crate::robot::control_types::{ CartesianPose, CartesianVelocities, ControllerMode, ConvertMotion, JointPositions, @@ -54,9 +54,9 @@ pub trait RobotWrapper { /// /// This minimal example will print the robot state 100 times: /// ```no_run - /// use franka::{FR3, RobotState, RobotWrapper, FrankaResult}; + /// use franka::{Fr3, RobotState, RobotWrapper, FrankaResult}; /// fn main() -> FrankaResult<()> { - /// let mut robot = FR3::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); @@ -1168,10 +1168,10 @@ impl PrivateRobot for Panda { } } -pub struct FR3 { - robimpl: RobotImplGeneric, +pub struct Fr3 { + robimpl: RobotImplGeneric, } -impl PrivateRobot for FR3 { +impl PrivateRobot for Fr3 { fn get_rob_mut(&mut self) -> &mut Self::Rob { &mut self.robimpl } @@ -1183,17 +1183,17 @@ impl PrivateRobot for FR3 { &mut self.robimpl.network } } -impl Robot for FR3 { - type Data = FR3Data; +impl Robot for Fr3 { + type Data = Fr3Data; type Rob = RobotImplGeneric; } -impl FR3 { +impl Fr3 { pub fn new>, LogSize: Into>>( franka_address: &str, realtime_config: RtConfig, log_size: LogSize, - ) -> FrankaResult { + ) -> 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(|_| { @@ -1201,8 +1201,8 @@ impl FR3 { message: "Connection could not be established".to_string(), } })?; - Ok(FR3 { - robimpl: ::Rob::new(network, log_size, realtime_config)?, + Ok(Fr3 { + robimpl: ::Rob::new(network, log_size, realtime_config)?, }) } } @@ -1339,15 +1339,15 @@ mod tests { use crate::exception::FrankaException; use crate::network::MessageCommand; use crate::robot::service_types::{ - ConnectRequestWithPandaHeader, ConnectResponsePanda, ConnectStatus, FR3CommandEnum, - FR3CommandHeader, GetterSetterStatusPanda, MoveControllerMode, MoveDeviation, + ConnectRequestWithPandaHeader, ConnectResponsePanda, ConnectStatus, Fr3CommandEnum, + Fr3CommandHeader, GetterSetterStatusPanda, MoveControllerMode, MoveDeviation, MoveMotionGeneratorMode, MoveRequest, MoveRequestWithPandaHeader, MoveStatusPanda, PandaCommandEnum, PandaCommandHeader, SetCollisionBehaviorRequest, - SetCollisionBehaviorRequestWithFR3Header, SetCollisionBehaviorRequestWithPandaHeader, - SetterResponseFR3, COMMAND_PORT, FR3_VERSION, + SetCollisionBehaviorRequestWithFr3Header, SetCollisionBehaviorRequestWithPandaHeader, + SetterResponseFr3, COMMAND_PORT, FR3_VERSION, }; use crate::robot::types::PandaStateIntern; - use crate::robot::{RobotWrapper, FR3}; + use crate::robot::{Fr3, RobotWrapper}; use crate::{FrankaResult, JointPositions, MotionFinished, Panda, RealtimeConfig, RobotState}; use bincode::{deserialize, serialize, serialized_size}; use std::iter::FromIterator; @@ -1534,11 +1534,11 @@ mod tests { lower_force_thresholds_nominal: [f64; 6], upper_force_thresholds_nominal: [f64; 6]| { counter += 1; - SetCollisionBehaviorRequestWithFR3Header { - header: FR3CommandHeader::new( - FR3CommandEnum::SetCollisionBehavior, + SetCollisionBehaviorRequestWithFr3Header { + header: Fr3CommandHeader::new( + Fr3CommandEnum::SetCollisionBehavior, counter, - size_of::() as u32, + size_of::() as u32, ), request: SetCollisionBehaviorRequest::new( lower_torque_thresholds_acceleration, @@ -1577,9 +1577,9 @@ mod tests { let req: SetCollisionBehaviorRequestWithPandaHeader = deserialize(&bytes).unwrap(); counter += 1; - let mut response = SetterResponseFR3 { - header: FR3CommandHeader::new( - FR3CommandEnum::SetCollisionBehavior, + let mut response = SetterResponseFr3 { + header: Fr3CommandHeader::new( + Fr3CommandEnum::SetCollisionBehavior, req.header.command_id, 0, ), @@ -1594,7 +1594,7 @@ mod tests { }); { std::thread::sleep(Duration::from_secs_f64(0.01)); - let mut robot = FR3::new("127.0.0.1", None, None).expect("robot failure"); + 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 @@ -1662,7 +1662,7 @@ mod tests { { std::thread::sleep(Duration::from_secs_f64(0.01)); let mut robot = - FR3::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( |_, _| { @@ -1727,7 +1727,7 @@ mod tests { { std::thread::sleep(Duration::from_secs_f64(0.01)); - let mut robot = FR3::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(); @@ -1746,7 +1746,7 @@ mod tests { }); { std::thread::sleep(Duration::from_secs_f64(0.01)); - let mut robot = FR3::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/service_types.rs b/src/robot/service_types.rs index 5ed5dfc..761ec50 100644 --- a/src/robot/service_types.rs +++ b/src/robot/service_types.rs @@ -34,7 +34,7 @@ macro_rules! define_fr3_request_with_header { #[derive(Serialize, Deserialize, Debug, Copy, Clone)] #[repr(packed)] pub struct $name { - pub header: FR3CommandHeader, + pub header: Fr3CommandHeader, pub request: $request, } impl MessageCommand for $name { @@ -46,7 +46,7 @@ macro_rules! define_fr3_request_with_header { fn from(tuple: (u32, $request)) -> Self { let command_id = tuple.0; $name { - header: FR3CommandHeader::new( + header: Fr3CommandHeader::new( $command, command_id, std::mem::size_of::() as u32, @@ -74,15 +74,15 @@ pub trait RobotHeader: MessageCommand + Serialize + Debug + Copy + Clone {} impl RobotHeader for PandaCommandHeader {} -impl MessageCommand for FR3CommandHeader { +impl MessageCommand for Fr3CommandHeader { fn get_command_message_id(&self) -> u32 { self.command_id } } -impl RobotHeader for FR3CommandHeader {} +impl RobotHeader for Fr3CommandHeader {} -impl CommandHeader for FR3CommandHeader { +impl CommandHeader for Fr3CommandHeader { fn get_command_id(&self) -> u32 { self.command_id } @@ -123,7 +123,7 @@ pub enum PandaCommandEnum { #[derive(Serialize_repr, Deserialize_repr, Debug, Copy, Clone)] #[repr(u32)] -pub enum FR3CommandEnum { +pub enum Fr3CommandEnum { Connect, Move, StopMove, @@ -169,7 +169,7 @@ pub enum MoveStatusPanda { #[derive(Serialize_repr, Deserialize_repr, Debug, Copy, Clone, PartialEq)] #[repr(u8)] -pub enum MoveStatusFR3 { +pub enum MoveStatusFr3 { Success, MotionStarted, Preempted, @@ -196,7 +196,7 @@ pub enum StopMoveStatusPanda { #[derive(Serialize_repr, Deserialize_repr, Debug, Copy, Clone)] #[repr(u8)] -pub enum StopMoveStatusFR3 { +pub enum StopMoveStatusFr3 { Success, CommandNotPossibleRejected, CommandRejectedDueToActivatedSafetyFunctions, @@ -218,7 +218,7 @@ pub enum AutomaticErrorRecoveryStatusPanda { #[derive(Serialize_repr, Deserialize_repr, Debug, Copy, Clone)] #[repr(u8)] -pub enum AutomaticErrorRecoveryStatusFR3 { +pub enum AutomaticErrorRecoveryStatusFr3 { Success, CommandNotPossibleRejected, CommandRejectedDueToActivatedSafetyFunctions, @@ -245,7 +245,7 @@ pub enum GetterSetterStatusPanda { #[derive(Serialize_repr, Deserialize_repr, Debug, Copy, Clone)] #[repr(u8)] -pub enum GetterSetterStatusFR3 { +pub enum GetterSetterStatusFr3 { Success, CommandNotPossibleRejected, InvalidArgumentRejected, @@ -271,9 +271,9 @@ define_panda_request_with_header!( PandaCommandEnum::Connect ); define_fr3_request_with_header!( - ConnectRequestWithFR3Header, + ConnectRequestWithFr3Header, ConnectRequest, - FR3CommandEnum::Connect + Fr3CommandEnum::Connect ); #[derive(Serialize, Deserialize)] @@ -346,7 +346,7 @@ define_panda_request_with_header!( MoveRequest, PandaCommandEnum::Move ); -define_fr3_request_with_header!(MoveRequestWithFR3Header, MoveRequest, FR3CommandEnum::Move); +define_fr3_request_with_header!(MoveRequestWithFr3Header, MoveRequest, Fr3CommandEnum::Move); #[derive(Serialize, Deserialize, Debug, Copy, Clone)] #[repr(packed)] @@ -356,8 +356,8 @@ pub struct StopMoveRequestWithPandaHeader { #[derive(Serialize, Deserialize, Debug, Copy, Clone)] #[repr(packed)] -pub struct StopMoveRequestWithFR3Header { - pub header: FR3CommandHeader, +pub struct StopMoveRequestWithFr3Header { + pub header: Fr3CommandHeader, } impl MessageCommand for StopMoveRequestWithPandaHeader { @@ -435,9 +435,9 @@ define_panda_request_with_header!( PandaCommandEnum::SetCollisionBehavior ); define_fr3_request_with_header!( - SetCollisionBehaviorRequestWithFR3Header, + SetCollisionBehaviorRequestWithFr3Header, SetCollisionBehaviorRequest, - FR3CommandEnum::SetCollisionBehavior + Fr3CommandEnum::SetCollisionBehavior ); #[derive(Serialize, Deserialize, Debug, Copy, Clone)] @@ -460,9 +460,9 @@ define_panda_request_with_header!( PandaCommandEnum::SetJointImpedance ); define_fr3_request_with_header!( - SetJointImpedanceRequestWithFR3Header, + SetJointImpedanceRequestWithFr3Header, SetJointImpedanceRequest, - FR3CommandEnum::SetJointImpedance + Fr3CommandEnum::SetJointImpedance ); #[derive(Serialize, Deserialize, Debug, Copy, Clone)] @@ -485,9 +485,9 @@ define_panda_request_with_header!( PandaCommandEnum::SetCartesianImpedance ); define_fr3_request_with_header!( - SetCartesianImpedanceRequestWithFR3Header, + SetCartesianImpedanceRequestWithFr3Header, SetCartesianImpedanceRequest, - FR3CommandEnum::SetCartesianImpedance + Fr3CommandEnum::SetCartesianImpedance ); #[derive(Serialize, Deserialize, Debug, Copy, Clone)] @@ -514,9 +514,9 @@ define_panda_request_with_header!( PandaCommandEnum::SetGuidingMode ); define_fr3_request_with_header!( - SetGuidingModeRequestWithFR3Header, + SetGuidingModeRequestWithFr3Header, SetGuidingModeRequest, - FR3CommandEnum::SetGuidingMode + Fr3CommandEnum::SetGuidingMode ); #[derive(Serialize, Deserialize, Debug, Copy, Clone)] @@ -539,9 +539,9 @@ define_panda_request_with_header!( PandaCommandEnum::SetEeToK ); define_fr3_request_with_header!( - SetEeToKRequestWithFR3Header, + SetEeToKRequestWithFr3Header, SetEeToKRequest, - FR3CommandEnum::SetEeToK + Fr3CommandEnum::SetEeToK ); #[derive(Serialize, Deserialize, Debug, Copy, Clone)] @@ -564,9 +564,9 @@ define_panda_request_with_header!( PandaCommandEnum::SetNeToEe ); define_fr3_request_with_header!( - SetNeToEeRequestWithFR3Header, + SetNeToEeRequestWithFr3Header, SetNeToEeRequest, - FR3CommandEnum::SetNeToEe + Fr3CommandEnum::SetNeToEe ); #[derive(Serialize, Deserialize, Debug, Copy, Clone)] @@ -595,9 +595,9 @@ define_panda_request_with_header!( PandaCommandEnum::SetLoad ); define_fr3_request_with_header!( - SetLoadRequestWithFR3Header, + SetLoadRequestWithFr3Header, SetLoadRequest, - FR3CommandEnum::SetLoad + Fr3CommandEnum::SetLoad ); #[derive(Serialize, Deserialize, Debug, Copy, Clone)] @@ -645,8 +645,8 @@ pub struct SetterResponse { } #[derive(Serialize, Deserialize, Debug)] -pub struct SetterResponseFR3 { - pub header: FR3CommandHeader, +pub struct SetterResponseFr3 { + pub header: Fr3CommandHeader, pub status: GetterSetterStatusPanda, } @@ -679,9 +679,9 @@ define_panda_request_with_header!( PandaCommandEnum::LoadModelLibrary ); define_fr3_request_with_header!( - LoadModelLibraryRequestWithFR3Header, + LoadModelLibraryRequestWithFr3Header, LoadModelLibraryRequest, - FR3CommandEnum::LoadModelLibrary + Fr3CommandEnum::LoadModelLibrary ); #[derive(Serialize, Deserialize, Debug, Copy, Clone)] @@ -714,15 +714,15 @@ impl PandaCommandHeader { #[derive(Serialize, Deserialize, Debug, Copy, Clone)] #[repr(packed)] -pub struct FR3CommandHeader { - pub command: FR3CommandEnum, +pub struct Fr3CommandHeader { + pub command: Fr3CommandEnum, pub command_id: u32, pub size: u32, } -impl FR3CommandHeader { - pub fn new(command: FR3CommandEnum, command_id: u32, size: u32) -> FR3CommandHeader { - FR3CommandHeader { +impl Fr3CommandHeader { + pub fn new(command: Fr3CommandEnum, command_id: u32, size: u32) -> Fr3CommandHeader { + Fr3CommandHeader { command, command_id, size, diff --git a/src/robot/types.rs b/src/robot/types.rs index 755cae7..3e602ad 100644 --- a/src/robot/types.rs +++ b/src/robot/types.rs @@ -91,7 +91,7 @@ pub struct PandaStateIntern { pub control_command_success_rate: f64, } -pub type FR3StateIntern = PandaStateIntern; +pub type Fr3StateIntern = PandaStateIntern; pub trait AbstractRobotStateIntern: Copy + Clone + PartialEq { fn get_message_id(&self) -> u64; From 9decd361144f74fb46d5e763599f6fc91f8d5478 Mon Sep 17 00:00:00 2001 From: Marco Boneberger Date: Fri, 21 Apr 2023 16:04:22 +0200 Subject: [PATCH 26/63] move robot types to separtate files --- src/gripper.rs | 23 +- src/lib.rs | 6 +- src/network.rs | 486 +------------ src/robot.rs | 1314 +----------------------------------- src/robot/fr3.rs | 299 ++++++++ src/robot/panda.rs | 440 ++++++++++++ src/robot/robot.rs | 89 +++ src/robot/robot_wrapper.rs | 985 +++++++++++++++++++++++++++ 8 files changed, 1845 insertions(+), 1797 deletions(-) create mode 100644 src/robot/fr3.rs create mode 100644 src/robot/panda.rs create mode 100644 src/robot/robot.rs create mode 100644 src/robot/robot_wrapper.rs diff --git a/src/gripper.rs b/src/gripper.rs index 5e974a4..13f1478 100644 --- a/src/gripper.rs +++ b/src/gripper.rs @@ -12,7 +12,7 @@ use crate::gripper::types::{ GraspRequestWithHeader, GripperCommandEnum, GripperCommandHeader, GripperStateIntern, MoveRequest, MoveRequestWithHeader, Status, COMMAND_PORT, GRIPPER_VERSION, }; -use crate::network::{GripperData, Network}; +use crate::network::{DeviceData, Network}; pub mod gripper_state; pub(crate) mod types; @@ -649,3 +649,24 @@ mod tests { thread.join().unwrap(); } } + +pub struct GripperData {} + +impl DeviceData for GripperData { + 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/lib.rs b/src/lib.rs index 75d8bec..470ccf5 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -180,10 +180,10 @@ pub use model::PandaModel; pub use model::RobotModel; pub use network::RobotData; pub use robot::control_types::*; +pub use robot::fr3::Fr3; pub use robot::low_pass_filter::DEFAULT_CUTOFF_FREQUENCY; pub use robot::low_pass_filter::MAX_CUTOFF_FREQUENCY; +pub use robot::panda::Panda; pub use robot::robot_state::RobotState; -pub use robot::Fr3; -pub use robot::Panda; -pub use robot::RobotWrapper; +pub use robot::robot_wrapper::RobotWrapper; pub use utils::*; diff --git a/src/network.rs b/src/network.rs index c7aa710..f5e5de2 100644 --- a/src/network.rs +++ b/src/network.rs @@ -51,8 +51,8 @@ use crate::robot::service_types::{ SetNeToEeRequestWithFr3Header, SetNeToEeRequestWithPandaHeader, StopMoveStatusFr3, StopMoveStatusPanda, FR3_VERSION, PANDA_VERSION, }; -use crate::robot::types::{AbstractRobotStateIntern, Fr3StateIntern, PandaStateIntern}; -use crate::{Fr3Model, PandaModel, RobotModel, RobotState}; +use crate::robot::types::AbstractRobotStateIntern; +use crate::{RobotModel, RobotState}; const CLIENT: Token = Token(1); @@ -204,488 +204,6 @@ pub trait RobotData: DeviceData { -> Result<(), FrankaException>; } -pub struct PandaData {} - -pub struct Fr3Data {} - -pub struct GripperData {} - -impl DeviceData for PandaData { - 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 PandaData { - type DeviceData = Self; - type Header = PandaCommandHeader; - type State = RobotState; - type StateIntern = PandaStateIntern; - type Model = PandaModel; - 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!", - )), - } - } -} -impl DeviceData for Fr3Data { - 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 RobotData for Fr3Data { - type DeviceData = Self; - type Header = Fr3CommandHeader; - type State = RobotState; - type StateIntern = Fr3StateIntern; - type Model = Fr3Model; - 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.", - )) - } - } -} - -impl DeviceData for GripperData { - 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 - } -} - pub trait MessageCommand { fn get_command_message_id(&self) -> u32; } diff --git a/src/robot.rs b/src/robot.rs index dc38824..b851d0e 100644 --- a/src/robot.rs +++ b/src/robot.rs @@ -2,1331 +2,27 @@ // 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; - -use crate::exception::{create_command_exception, FrankaException, FrankaResult}; -use crate::network::{Fr3Data, Network, PandaData, RobotData}; -use crate::robot::control_loop::ControlLoop; -use crate::robot::control_types::{ - CartesianPose, CartesianVelocities, ControllerMode, ConvertMotion, 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::robot::robot_control::RobotControl; -use crate::robot::robot_impl::{RobotImplGeneric, RobotImplementation}; -use crate::robot::service_types::{ - GetCartesianLimitRequest, GetCartesianLimitRequestWithPandaHeader, GetCartesianLimitResponse, - GetterSetterStatusPanda, PandaCommandEnum, SetCartesianImpedanceRequest, - SetCollisionBehaviorRequest, SetEeToKRequest, SetFiltersRequest, - SetFiltersRequestWithPandaHeader, SetFiltersResponse, SetGuidingModeRequest, - SetJointImpedanceRequest, SetLoadRequest, SetNeToEeRequest, StopMoveStatusPanda, -}; -use crate::robot::virtual_wall_cuboid::VirtualWallCuboid; -use crate::utils::MotionGenerator; -use crate::{Finishable, RobotModel, RobotState}; mod control_loop; mod control_tools; pub mod control_types; pub mod error; pub mod errors; +pub mod fr3; pub mod logger; pub mod low_pass_filter; mod motion_generator_traits; +pub mod panda; mod rate_limiting; +pub mod robot; mod robot_control; mod robot_impl; pub mod robot_state; +pub mod robot_wrapper; pub(crate) mod service_types; pub(crate) mod types; pub mod virtual_wall_cuboid; -pub trait RobotWrapper { - type Model: RobotModel; - - /// 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::{Fr3, RobotState, RobotWrapper, FrankaResult}; - /// fn main() -> FrankaResult<()> { - /// let mut robot = Fr3::new("robotik-bs.de",None,None)?; - /// let mut count = 0; - /// robot.read(| robot_state:&RobotState | -> bool { - /// println!("{:?}", robot_state); - /// count += 1; - /// count <= 100 - /// }) - /// } - /// ``` - /// # 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 - /// * [`NetworkException`](`crate::exception::FrankaException::NetworkException`) if the connection is lost, e.g. after a timeout. - fn read bool>(&mut self, read_callback: F) -> 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` 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. - 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<()>; - - /// 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<()>; - - /// Waits for a robot state update and returns it. - /// - /// # 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; - - /// Changes the collision behavior. - /// - /// Set separate torque and force boundaries for acceleration/deceleration and constant velocity - /// movement phases. - /// - /// Forces or torques between lower and upper threshold are shown as contacts in the RobotState. - /// Forces or torques above the upper threshold are registered as collision and cause the robot to - /// stop moving. - /// - /// # Arguments - /// * `lower_torque_thresholds_acceleration` - Contact torque thresholds during - /// acceleration/deceleration for each joint in \[Nm\] - /// * `upper_torque_thresholds_acceleration` - Collision torque thresholds during - /// acceleration/deceleration for each joint in \[Nm\] - /// * `lower_torque_thresholds_nominal` - Contact torque thresholds for each joint in \[Nm\] - /// * `upper_torque_thresholds_nominal` - Collision torque thresholds for each joint in \[Nm\] - /// * `lower_force_thresholds_acceleration` -Contact force thresholds during - /// acceleration/deceleration for (x,y,z,R,P,Y) in \[N\]. - /// * `upper_force_thresholds_acceleration` - Collision force thresholds during - /// acceleration/deceleration for (x,y,z,R,P,Y) in \[N\]. - /// * `lower_force_thresholds_nominal` - Contact force thresholds for (x,y,z,R,P,Y) in \[N\] - /// * `upper_force_thresholds_nominal` - Collision force thresholds for (x,y,z,R,P,Y) in \[N\] - /// # 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. - /// # See also - /// * [`RobotState::cartesian_contact`](`crate::RobotState::cartesian_contact`) - /// * [`RobotState::cartesian_collision`](`crate::RobotState::cartesian_collision`) - /// * [`RobotState::joint_contact`](`crate::RobotState::joint_contact`) - /// * [`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)] - fn set_collision_behavior( - &mut self, - 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<()>; - - /// 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. - /// - /// User-provided torques are not affected by this setting. - /// # Arguments - /// * `K_theta` - Joint impedance values ![K_{\theta}](https://latex.codecogs.com/png.latex?K_{\theta}). - /// # 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_joint_impedance(&mut self, K_theta: [f64; 7]) -> FrankaResult<()>; - - /// Sets the Cartesian impedance for (x, y, z, roll, pitch, yaw) in the internal controller. - /// - /// User-provided torques are not affected by this setting. - /// # Arguments - /// * `K_x` - Cartesian impedance values - /// - /// ![K_x=(x \in \[10,3000\] \frac{N}{m}, y \in \[10,3000\] \frac{N}{m}, z \in \[10,3000\] \frac{N}{m}, R \in \[1,300\] \frac{Nm}{rad}, P \in \[1,300\] \frac{Nm}{rad}, Y \in \[1,300\] \frac{Nm}{rad})](https://latex.codecogs.com/png.latex?K_x=(x&space;\in&space;[10,3000]&space;\frac{N}{m},&space;y&space;\in&space;[10,3000]&space;\frac{N}{m},&space;z&space;\in&space;[10,3000]&space;\frac{N}{m},&space;R&space;\in&space;[1,300]&space;\frac{Nm}{rad},&space;P&space;\in&space;[1,300]&space;\frac{Nm}{rad},&space;Y&space;\in&space;[1,300]&space;\frac{Nm}{rad})) - /// # 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_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. - /// # Note - /// Guiding mode can be enabled by pressing the two opposing buttons near the robot's flange. - /// # Arguments - /// * `guiding_mode` - Unlocked movement in (x, y, z, R, P, Y) in guiding mode. - /// * `elbow` - True if the elbow is free in guiding mode, false otherwise. - /// # 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_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. - /// - /// The transformation matrix is represented as a vectorized 4x4 matrix in column-major format. - /// # Arguments - /// * `EE_T_K` - Vectorized EE-to-K transformation matrix ![^{EE}T_K](https://latex.codecogs.com/png.latex?^{EE}T_K), 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. - /// - /// See [Stiffness frame K](#stiffness-frame-k) for an explanation of the stiffness frame. - #[allow(non_snake_case)] - 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. - /// - /// The transformation matrix is represented as a vectorized 4x4 matrix in column-major format. - /// # Arguments - /// * `NE_T_EE` - Vectorized NE-to-EE transformation matrix ![^{NE}T_{EE}](https://latex.codecogs.com/png.latex?^{NE}T_{EE}), 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. - /// - /// # See also - /// * [`RobotState::NE_T_EE`](`crate::RobotState::NE_T_EE`) - /// * [`RobotState::O_T_EE`](`crate::RobotState::O_T_EE`) - /// * [`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)] - fn set_EE(&mut self, NE_T_EE: [f64; 16]) -> FrankaResult<()>; - - ///Runs automatic error recovery on the robot. - /// - /// Automatic error recovery e.g. resets the robot after a collision occurred. - /// # 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 automatic_error_recovery(&mut self) -> FrankaResult<()>; - - /// Stops all currently running motions. - /// - /// If a control or motion generator loop is running in another thread, it will be preempted - /// with a [`ControlException`](`crate::exception::FrankaException::ControlException`). - /// # 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 stop(&mut self) -> 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` 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. - 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` 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. - 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` 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. - 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. 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. - 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 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. - 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 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. - 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 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. - 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` 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. - fn control_torques_and_cartesian_velocities< - F: FnMut(&RobotState, &Duration) -> CartesianVelocities, - 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<()>; - - /// 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. - fn load_model(&mut self, persistent: bool) -> FrankaResult; - - /// Returns the software version reported by the connected server. - /// - /// # Return - /// Software version of the connected server. - fn server_version(&self) -> u16; -} - -impl RobotWrapper for R -where - RobotState: From<<::Data as RobotData>::State>, - CartesianPose: control_types::ConvertMotion<<::Data as RobotData>::State>, - JointVelocities: control_types::ConvertMotion<<::Data as RobotData>::State>, - JointPositions: control_types::ConvertMotion<<::Data as RobotData>::State>, - CartesianVelocities: control_types::ConvertMotion<<::Data as RobotData>::State>, -{ - type Model = <::Data as RobotData>::Model; - - 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(()) - } - - fn control_joint_positions< - F: FnMut(&RobotState, &Duration) -> JointPositions, - CM: Into>, - L: Into>, - CF: Into>, - >( - &mut self, - mut motion_generator_callback: F, - controller_mode: CM, - limit_rate: L, - cutoff_frequency: CF, - ) -> FrankaResult<()> { - let cb = |state: &<::Data as RobotData>::State, duration: &Duration| { - motion_generator_callback(&(state.clone().into()), duration) - }; - ::control_motion_intern( - self, - cb, - controller_mode.into(), - limit_rate.into(), - cutoff_frequency.into(), - ) - } - - 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 read_once(&mut self) -> FrankaResult { - match ::get_rob_mut(self).read_once() { - Ok(state) => Ok(state.into()), - Err(e) => Err(e), - } - } - - #[allow(clippy::too_many_arguments)] - fn set_collision_behavior( - &mut self, - 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 command = ::Data::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); - ::Data::handle_getter_setter_status(status) - } - - 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], - )?; - self.set_joint_impedance([3000., 3000., 3000., 2500., 2500., 2000., 2000.])?; - self.set_cartesian_impedance([3000., 3000., 3000., 300., 300., 300.])?; - Ok(()) - } - - #[allow(non_snake_case)] - fn set_joint_impedance(&mut self, K_theta: [f64; 7]) -> FrankaResult<()> { - let command = ::Data::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); - ::Data::handle_getter_setter_status(status) - } - - #[allow(non_snake_case)] - fn set_cartesian_impedance(&mut self, K_x: [f64; 6]) -> FrankaResult<()> { - let command = ::Data::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); - ::Data::handle_getter_setter_status(status) - } - - #[allow(non_snake_case)] - fn set_load( - &mut self, - load_mass: f64, - F_x_Cload: [f64; 3], - load_inertia: [f64; 9], - ) -> FrankaResult<()> { - let command = ::Data::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); - ::Data::handle_getter_setter_status(status) - } - - fn set_guiding_mode(&mut self, guiding_mode: [bool; 6], elbow: bool) -> FrankaResult<()> { - let command = ::Data::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); - ::Data::handle_getter_setter_status(status) - } - - #[allow(non_snake_case)] - fn set_K(&mut self, EE_T_K: [f64; 16]) -> FrankaResult<()> { - let command = ::Data::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); - ::Data::handle_getter_setter_status(status) - } - - #[allow(non_snake_case)] - fn set_EE(&mut self, NE_T_EE: [f64; 16]) -> FrankaResult<()> { - let command = ::Data::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); - ::Data::handle_getter_setter_status(status) - } - - fn automatic_error_recovery(&mut self) -> FrankaResult<()> { - let command = ::Data::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); - ::Data::handle_automatic_error_recovery_status(status) - } - - fn stop(&mut self) -> FrankaResult<()> { - let command = ::Data::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 control_joint_velocities< - F: FnMut(&RobotState, &Duration) -> JointVelocities, - CM: Into>, - L: Into>, - CF: Into>, - >( - &mut self, - mut motion_generator_callback: F, - controller_mode: CM, - limit_rate: L, - cutoff_frequency: CF, - ) -> FrankaResult<()> { - let cb = |state: &<::Data as RobotData>::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(), - ) - } - - fn control_cartesian_pose< - F: FnMut(&RobotState, &Duration) -> CartesianPose, - CM: Into>, - L: Into>, - CF: Into>, - >( - &mut self, - mut motion_generator_callback: F, - controller_mode: CM, - limit_rate: L, - cutoff_frequency: CF, - ) -> FrankaResult<()> { - let cb = |state: &<::Data as RobotData>::State, duration: &Duration| { - motion_generator_callback(&(state.clone().into()), duration) - }; - ::control_motion_intern( - self, - cb, - controller_mode.into(), - limit_rate.into(), - cutoff_frequency.into(), - ) - } - - fn control_cartesian_velocities< - F: FnMut(&RobotState, &Duration) -> CartesianVelocities, - CM: Into>, - L: Into>, - CF: Into>, - >( - &mut self, - mut motion_generator_callback: F, - controller_mode: CM, - limit_rate: L, - cutoff_frequency: CF, - ) -> FrankaResult<()> { - let cb = |state: &<::Data as RobotData>::State, duration: &Duration| { - motion_generator_callback(&(state.clone().into()), duration) - }; - self.control_motion_intern( - cb, - controller_mode.into(), - limit_rate.into(), - cutoff_frequency.into(), - ) - } - - fn control_torques< - T: FnMut(&RobotState, &Duration) -> Torques, - L: Into>, - CF: Into>, - >( - &mut self, - mut control_callback: T, - limit_rate: L, - cutoff_frequency: CF, - ) -> FrankaResult<()> { - let motion_generator_callback = - |_state: &<::Data as RobotData>::State, _time_step: &Duration| { - JointVelocities::new([0.; 7]) - }; - let mut cb = |state: &<::Data as RobotData>::State, duration: &Duration| { - control_callback(&(state.clone().into()), duration) - }; - self.control_torques_intern( - &motion_generator_callback, - &mut cb, - limit_rate.into(), - cutoff_frequency.into(), - ) - } - - 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, - mut motion_generator_callback: F, - limit_rate: L, - cutoff_frequency: CF, - ) -> FrankaResult<()> { - let motion_generator_cb = |state: &<::Data as RobotData>::State, - duration: &Duration| { - motion_generator_callback(&(state.clone().into()), duration) - }; - let mut control_cb = |state: &<::Data as RobotData>::State, - duration: &Duration| { - control_callback(&(state.clone().into()), duration) - }; - self.control_torques_intern( - motion_generator_cb, - &mut control_cb, - limit_rate.into(), - cutoff_frequency.into(), - ) - } - - fn control_torques_and_joint_positions< - F: FnMut(&RobotState, &Duration) -> JointPositions, - 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_cb = |state: &<::Data as RobotData>::State, - duration: &Duration| { - motion_generator_callback(&(state.clone().into()), duration) - }; - let mut control_cb = |state: &<::Data as RobotData>::State, - duration: &Duration| { - control_callback(&(state.clone().into()), duration) - }; - self.control_torques_intern( - motion_generator_cb, - &mut control_cb, - limit_rate.into(), - cutoff_frequency.into(), - ) - } - - 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, - mut motion_generator_callback: F, - limit_rate: L, - cutoff_frequency: CF, - ) -> FrankaResult<()> { - let motion_generator_cb = |state: &<::Data as RobotData>::State, - duration: &Duration| { - motion_generator_callback(&(state.clone().into()), duration) - }; - let mut control_cb = |state: &<::Data as RobotData>::State, - duration: &Duration| { - control_callback(&(state.clone().into()), duration) - }; - self.control_torques_intern( - motion_generator_cb, - &mut control_cb, - limit_rate.into(), - cutoff_frequency.into(), - ) - } - - 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_cb = |state: &<::Data as RobotData>::State, - duration: &Duration| { - motion_generator_callback(&(state.clone().into()), duration) - }; - let mut control_cb = |state: &<::Data as RobotData>::State, - duration: &Duration| { - control_callback(&(state.clone().into()), duration) - }; - self.control_torques_intern( - motion_generator_cb, - &mut control_cb, - limit_rate.into(), - cutoff_frequency.into(), - ) - } - - fn load_model(&mut self, persistent: bool) -> FrankaResult { - ::get_rob_mut(self).load_model(persistent) - } - - fn server_version(&self) -> u16 { - ::get_rob(self).server_version() - } -} - -trait PrivateRobot: Robot -where - CartesianPose: ConvertMotion<<::Data as RobotData>::State>, - JointVelocities: ConvertMotion<<::Data as RobotData>::State>, - JointPositions: ConvertMotion<<::Data as RobotData>::State>, - CartesianVelocities: ConvertMotion<<::Data as RobotData>::State>, - RobotState: From<<::Data as RobotData>::State>, -{ - 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(&<::Data as RobotData>::State, &Duration) -> U, - U: ConvertMotion<<::Data as RobotData>::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(true); - 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(&<::Data as RobotData>::State, &Duration) -> U, - U: ConvertMotion<<::Data as RobotData>::State> - + Debug - + MotionGeneratorTrait - + Finishable, - >( - &mut self, - motion_generator_callback: F, - control_callback: &mut dyn FnMut( - &<::Data as RobotData>::State, - &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( - self.get_rob_mut(), - control_callback, - motion_generator_callback, - limit_rate, - cutoff_frequency, - )?; - control_loop.run() - } -} - -pub trait Robot -where - CartesianPose: ConvertMotion<<::Data as RobotData>::State>, - JointVelocities: ConvertMotion<<::Data as RobotData>::State>, - JointPositions: ConvertMotion<<::Data as RobotData>::State>, - CartesianVelocities: ConvertMotion<<::Data as RobotData>::State>, - RobotState: From<<::Data as RobotData>::State>, -{ - type Data: RobotData; - type Rob: RobotImplementation; -} - -/// Maintains a network connection to the robot, provides the current robot state, gives access to -/// the model library and allows to control the robot. -/// # 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. -/// # 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`). -/// -/// -/// # Motion generation and joint-level torque commands -/// -/// The following methods allow to perform motion generation and/or send joint-level torque -/// 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. -/// -/// 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 -/// be called again with the most recently received robot state. Since the robot is controlled with -/// 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 -/// 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::robot::control_types::{JointPositions, MotionFinished}; -/// use std::time::Duration; -/// # fn your_function_which_generates_joint_positions(time:f64) -> JointPositions {JointPositions::new([0.;7])} -/// let mut time = 0.; -/// let callback = |state: &RobotState, time_step: &Duration| -> JointPositions { -/// time += time_step.as_secs_f64(); -/// let out: JointPositions = your_function_which_generates_joint_positions(time); -/// if time >= 5.0 { -/// return out.motion_finished(); -/// } -/// return out; -/// }; -/// ``` -/// # Commands -/// -/// 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 Panda { - robimpl: RobotImplGeneric, -} - -impl Robot for Panda { - type Data = PandaData; - type Rob = RobotImplGeneric; -} - -impl PrivateRobot for Panda { - fn get_rob_mut(&mut self) -> &mut Self::Rob { - &mut self.robimpl - } - fn get_rob(&self) -> &Self::Rob { - &self.robimpl - } - - fn get_net(&mut self) -> &mut Network { - &mut self.robimpl.network - } -} - -pub struct Fr3 { - robimpl: RobotImplGeneric, -} -impl PrivateRobot for Fr3 { - fn get_rob_mut(&mut self) -> &mut Self::Rob { - &mut self.robimpl - } - fn get_rob(&self) -> &Self::Rob { - &self.robimpl - } - - fn get_net(&mut self) -> &mut Network { - &mut self.robimpl.network - } -} -impl Robot for Fr3 { - type Data = Fr3Data; - type Rob = RobotImplGeneric; -} - -impl 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 { - robimpl: ::Rob::new(network, log_size, realtime_config)?, - }) - } -} - -impl Panda { - /// Establishes a connection with the 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`](`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(franka_address, service_types::COMMAND_PORT).map_err(|_| { - FrankaException::NetworkException { - message: "Connection could not be established".to_string(), - } - })?; - Ok(Panda { - robimpl: ::Rob::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.robimpl.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.robimpl.network.tcp_send_request(command); - let response: SetFiltersResponse = self - .robimpl - .network - .tcp_blocking_receive_response(command_id); - PandaData::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.robimpl.network.create_header_for_panda( - PandaCommandEnum::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 { - 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!", - )), - } - } -} - #[cfg(test)] mod tests { use mockall::{automock, predicate::*}; @@ -1347,7 +43,7 @@ mod tests { SetterResponseFr3, COMMAND_PORT, FR3_VERSION, }; use crate::robot::types::PandaStateIntern; - use crate::robot::{Fr3, RobotWrapper}; + use crate::{Fr3, RobotWrapper}; use crate::{FrankaResult, JointPositions, MotionFinished, Panda, RealtimeConfig, RobotState}; use bincode::{deserialize, serialize, serialized_size}; use std::iter::FromIterator; diff --git a/src/robot/fr3.rs b/src/robot/fr3.rs new file mode 100644 index 0000000..a610ab6 --- /dev/null +++ b/src/robot/fr3.rs @@ -0,0 +1,299 @@ +use crate::exception::{create_command_exception, FrankaException}; +use crate::network::{DeviceData, Network}; +use crate::robot::errors::FrankaErrors; +use crate::robot::logger::Record; +use crate::robot::robot::{PrivateRobot, Robot}; +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::{Fr3Model, FrankaResult, RealtimeConfig, RobotData, RobotState}; +use std::mem::size_of; + +pub struct Fr3 { + robimpl: RobotImplGeneric, +} + +impl PrivateRobot for Fr3 { + fn get_rob_mut(&mut self) -> &mut Self::Rob { + &mut self.robimpl + } + fn get_rob(&self) -> &Self::Rob { + &self.robimpl + } + + fn get_net(&mut self) -> &mut Network { + &mut self.robimpl.network + } +} + +impl Robot for Fr3 { + type Data = Fr3Data; + type Rob = RobotImplGeneric; +} + +impl 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 { + robimpl: ::Rob::new(network, log_size, realtime_config)?, + }) + } +} + +pub struct Fr3Data {} + +impl DeviceData for Fr3Data { + 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 RobotData for Fr3Data { + type DeviceData = Self; + type Header = Fr3CommandHeader; + type State = RobotState; + type StateIntern = Fr3StateIntern; + type Model = Fr3Model; + 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.", + )) + } + } +} diff --git a/src/robot/panda.rs b/src/robot/panda.rs new file mode 100644 index 0000000..9d5aae5 --- /dev/null +++ b/src/robot/panda.rs @@ -0,0 +1,440 @@ +use crate::exception::{create_command_exception, FrankaException}; +use crate::network::{DeviceData, Network}; +use crate::robot::errors::FrankaErrors; +use crate::robot::logger::Record; +use crate::robot::robot::{PrivateRobot, Robot}; +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, PandaModel, RealtimeConfig, RobotData, RobotState}; +use std::mem::size_of; + +/// Maintains a network connection to the robot, provides the current robot state, gives access to +/// the model library and allows to control the robot. +/// # 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. +/// # 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`). +/// +/// +/// # Motion generation and joint-level torque commands +/// +/// The following methods allow to perform motion generation and/or send joint-level torque +/// 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. +/// +/// 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 +/// be called again with the most recently received robot state. Since the robot is controlled with +/// 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 +/// 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::robot::control_types::{JointPositions, MotionFinished}; +/// use std::time::Duration; +/// # fn your_function_which_generates_joint_positions(time:f64) -> JointPositions {JointPositions::new([0.;7])} +/// let mut time = 0.; +/// let callback = |state: &RobotState, time_step: &Duration| -> JointPositions { +/// time += time_step.as_secs_f64(); +/// let out: JointPositions = your_function_which_generates_joint_positions(time); +/// if time >= 5.0 { +/// return out.motion_finished(); +/// } +/// return out; +/// }; +/// ``` +/// # Commands +/// +/// 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 Panda { + robimpl: RobotImplGeneric, +} + +impl Panda { + /// Establishes a connection with the 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`](`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(franka_address, service_types::COMMAND_PORT).map_err(|_| { + FrankaException::NetworkException { + message: "Connection could not be established".to_string(), + } + })?; + Ok(Panda { + robimpl: ::Rob::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.robimpl.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.robimpl.network.tcp_send_request(command); + let response: SetFiltersResponse = self + .robimpl + .network + .tcp_blocking_receive_response(command_id); + PandaData::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.robimpl.network.create_header_for_panda( + PandaCommandEnum::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 { + 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 Robot for Panda { + type Data = PandaData; + type Rob = RobotImplGeneric; +} + +impl PrivateRobot for Panda { + fn get_rob_mut(&mut self) -> &mut Self::Rob { + &mut self.robimpl + } + fn get_rob(&self) -> &Self::Rob { + &self.robimpl + } + + fn get_net(&mut self) -> &mut Network { + &mut self.robimpl.network + } +} + +pub struct PandaData {} + +impl DeviceData for PandaData { + 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 PandaData { + type DeviceData = Self; + type Header = PandaCommandHeader; + type State = RobotState; + type StateIntern = PandaStateIntern; + type Model = PandaModel; + 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/robot.rs b/src/robot/robot.rs new file mode 100644 index 0000000..55f7a0a --- /dev/null +++ b/src/robot/robot.rs @@ -0,0 +1,89 @@ +use crate::network::Network; +use crate::robot::control_loop::ControlLoop; +use crate::robot::motion_generator_traits::MotionGeneratorTrait; +use crate::robot::robot_impl::RobotImplementation; +use crate::{ + CartesianPose, CartesianVelocities, ControllerMode, ConvertMotion, Finishable, FrankaResult, + JointPositions, JointVelocities, RobotData, RobotState, Torques, DEFAULT_CUTOFF_FREQUENCY, +}; +use std::fmt::Debug; +use std::time::Duration; + +pub(crate) trait PrivateRobot: Robot +where + CartesianPose: ConvertMotion<<::Data as RobotData>::State>, + JointVelocities: ConvertMotion<<::Data as RobotData>::State>, + JointPositions: ConvertMotion<<::Data as RobotData>::State>, + CartesianVelocities: ConvertMotion<<::Data as RobotData>::State>, + RobotState: From<<::Data as RobotData>::State>, +{ + 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(&<::Data as RobotData>::State, &Duration) -> U, + U: ConvertMotion<<::Data as RobotData>::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(true); + 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(&<::Data as RobotData>::State, &Duration) -> U, + U: ConvertMotion<<::Data as RobotData>::State> + + Debug + + MotionGeneratorTrait + + Finishable, + >( + &mut self, + motion_generator_callback: F, + control_callback: &mut dyn FnMut( + &<::Data as RobotData>::State, + &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( + self.get_rob_mut(), + control_callback, + motion_generator_callback, + limit_rate, + cutoff_frequency, + )?; + control_loop.run() + } +} + +pub trait Robot +where + CartesianPose: ConvertMotion<<::Data as RobotData>::State>, + JointVelocities: ConvertMotion<<::Data as RobotData>::State>, + JointPositions: ConvertMotion<<::Data as RobotData>::State>, + CartesianVelocities: ConvertMotion<<::Data as RobotData>::State>, + RobotState: From<<::Data as RobotData>::State>, +{ + type Data: RobotData; + type Rob: RobotImplementation; +} diff --git a/src/robot/robot_wrapper.rs b/src/robot/robot_wrapper.rs new file mode 100644 index 0000000..b6d121f --- /dev/null +++ b/src/robot/robot_wrapper.rs @@ -0,0 +1,985 @@ +use crate::exception::create_command_exception; +use crate::robot::robot::PrivateRobot; +use crate::robot::robot::Robot; +use crate::robot::robot_control::RobotControl; +use crate::robot::robot_impl::RobotImplementation; +use crate::robot::service_types::{ + SetCartesianImpedanceRequest, SetCollisionBehaviorRequest, SetEeToKRequest, + SetGuidingModeRequest, SetJointImpedanceRequest, SetLoadRequest, SetNeToEeRequest, + StopMoveStatusPanda, +}; +use crate::{ + CartesianPose, CartesianVelocities, ControllerMode, FrankaResult, JointPositions, + JointVelocities, MotionGenerator, RobotData, RobotModel, RobotState, Torques, + MAX_CUTOFF_FREQUENCY, +}; +use std::time::Duration; + +pub trait RobotWrapper { + type Model: RobotModel; + + /// 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::{Fr3, RobotState, RobotWrapper, FrankaResult}; + /// fn main() -> FrankaResult<()> { + /// let mut robot = Fr3::new("robotik-bs.de",None,None)?; + /// let mut count = 0; + /// robot.read(| robot_state:&RobotState | -> bool { + /// println!("{:?}", robot_state); + /// count += 1; + /// count <= 100 + /// }) + /// } + /// ``` + /// # 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 + /// * [`NetworkException`](`crate::exception::FrankaException::NetworkException`) if the connection is lost, e.g. after a timeout. + fn read bool>(&mut self, read_callback: F) -> 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` 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. + 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<()>; + + /// 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<()>; + + /// Waits for a robot state update and returns it. + /// + /// # 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; + + /// Changes the collision behavior. + /// + /// Set separate torque and force boundaries for acceleration/deceleration and constant velocity + /// movement phases. + /// + /// Forces or torques between lower and upper threshold are shown as contacts in the RobotState. + /// Forces or torques above the upper threshold are registered as collision and cause the robot to + /// stop moving. + /// + /// # Arguments + /// * `lower_torque_thresholds_acceleration` - Contact torque thresholds during + /// acceleration/deceleration for each joint in \[Nm\] + /// * `upper_torque_thresholds_acceleration` - Collision torque thresholds during + /// acceleration/deceleration for each joint in \[Nm\] + /// * `lower_torque_thresholds_nominal` - Contact torque thresholds for each joint in \[Nm\] + /// * `upper_torque_thresholds_nominal` - Collision torque thresholds for each joint in \[Nm\] + /// * `lower_force_thresholds_acceleration` -Contact force thresholds during + /// acceleration/deceleration for (x,y,z,R,P,Y) in \[N\]. + /// * `upper_force_thresholds_acceleration` - Collision force thresholds during + /// acceleration/deceleration for (x,y,z,R,P,Y) in \[N\]. + /// * `lower_force_thresholds_nominal` - Contact force thresholds for (x,y,z,R,P,Y) in \[N\] + /// * `upper_force_thresholds_nominal` - Collision force thresholds for (x,y,z,R,P,Y) in \[N\] + /// # 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. + /// # See also + /// * [`RobotState::cartesian_contact`](`crate::RobotState::cartesian_contact`) + /// * [`RobotState::cartesian_collision`](`crate::RobotState::cartesian_collision`) + /// * [`RobotState::joint_contact`](`crate::RobotState::joint_contact`) + /// * [`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)] + fn set_collision_behavior( + &mut self, + 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<()>; + + /// 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. + /// + /// User-provided torques are not affected by this setting. + /// # Arguments + /// * `K_theta` - Joint impedance values ![K_{\theta}](https://latex.codecogs.com/png.latex?K_{\theta}). + /// # 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_joint_impedance(&mut self, K_theta: [f64; 7]) -> FrankaResult<()>; + + /// Sets the Cartesian impedance for (x, y, z, roll, pitch, yaw) in the internal controller. + /// + /// User-provided torques are not affected by this setting. + /// # Arguments + /// * `K_x` - Cartesian impedance values + /// + /// ![K_x=(x \in \[10,3000\] \frac{N}{m}, y \in \[10,3000\] \frac{N}{m}, z \in \[10,3000\] \frac{N}{m}, R \in \[1,300\] \frac{Nm}{rad}, P \in \[1,300\] \frac{Nm}{rad}, Y \in \[1,300\] \frac{Nm}{rad})](https://latex.codecogs.com/png.latex?K_x=(x&space;\in&space;[10,3000]&space;\frac{N}{m},&space;y&space;\in&space;[10,3000]&space;\frac{N}{m},&space;z&space;\in&space;[10,3000]&space;\frac{N}{m},&space;R&space;\in&space;[1,300]&space;\frac{Nm}{rad},&space;P&space;\in&space;[1,300]&space;\frac{Nm}{rad},&space;Y&space;\in&space;[1,300]&space;\frac{Nm}{rad})) + /// # 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_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. + /// # Note + /// Guiding mode can be enabled by pressing the two opposing buttons near the robot's flange. + /// # Arguments + /// * `guiding_mode` - Unlocked movement in (x, y, z, R, P, Y) in guiding mode. + /// * `elbow` - True if the elbow is free in guiding mode, false otherwise. + /// # 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_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. + /// + /// The transformation matrix is represented as a vectorized 4x4 matrix in column-major format. + /// # Arguments + /// * `EE_T_K` - Vectorized EE-to-K transformation matrix ![^{EE}T_K](https://latex.codecogs.com/png.latex?^{EE}T_K), 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. + /// + /// See [Stiffness frame K](#stiffness-frame-k) for an explanation of the stiffness frame. + #[allow(non_snake_case)] + 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. + /// + /// The transformation matrix is represented as a vectorized 4x4 matrix in column-major format. + /// # Arguments + /// * `NE_T_EE` - Vectorized NE-to-EE transformation matrix ![^{NE}T_{EE}](https://latex.codecogs.com/png.latex?^{NE}T_{EE}), 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. + /// + /// # See also + /// * [`RobotState::NE_T_EE`](`crate::RobotState::NE_T_EE`) + /// * [`RobotState::O_T_EE`](`crate::RobotState::O_T_EE`) + /// * [`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)] + fn set_EE(&mut self, NE_T_EE: [f64; 16]) -> FrankaResult<()>; + + ///Runs automatic error recovery on the robot. + /// + /// Automatic error recovery e.g. resets the robot after a collision occurred. + /// # 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 automatic_error_recovery(&mut self) -> FrankaResult<()>; + + /// Stops all currently running motions. + /// + /// If a control or motion generator loop is running in another thread, it will be preempted + /// with a [`ControlException`](`crate::exception::FrankaException::ControlException`). + /// # 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 stop(&mut self) -> 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` 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. + 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` 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. + 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` 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. + 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. 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. + 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 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. + 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 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. + 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 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. + 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` 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. + fn control_torques_and_cartesian_velocities< + F: FnMut(&RobotState, &Duration) -> CartesianVelocities, + 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<()>; + + /// 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. + fn load_model(&mut self, persistent: bool) -> FrankaResult; + + /// Returns the software version reported by the connected server. + /// + /// # Return + /// Software version of the connected server. + fn server_version(&self) -> u16; +} + +impl RobotWrapper for R +where + RobotState: From<<::Data as RobotData>::State>, + CartesianPose: crate::ConvertMotion<<::Data as RobotData>::State>, + JointVelocities: crate::ConvertMotion<<::Data as RobotData>::State>, + JointPositions: crate::ConvertMotion<<::Data as RobotData>::State>, + CartesianVelocities: crate::ConvertMotion<<::Data as RobotData>::State>, +{ + type Model = <::Data as RobotData>::Model; + + 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(()) + } + + fn control_joint_positions< + F: FnMut(&RobotState, &Duration) -> JointPositions, + CM: Into>, + L: Into>, + CF: Into>, + >( + &mut self, + mut motion_generator_callback: F, + controller_mode: CM, + limit_rate: L, + cutoff_frequency: CF, + ) -> FrankaResult<()> { + let cb = |state: &<::Data as RobotData>::State, duration: &Duration| { + motion_generator_callback(&(state.clone().into()), duration) + }; + ::control_motion_intern( + self, + cb, + controller_mode.into(), + limit_rate.into(), + cutoff_frequency.into(), + ) + } + + 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 read_once(&mut self) -> FrankaResult { + match ::get_rob_mut(self).read_once() { + Ok(state) => Ok(state.into()), + Err(e) => Err(e), + } + } + + #[allow(clippy::too_many_arguments)] + fn set_collision_behavior( + &mut self, + 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 command = ::Data::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); + ::Data::handle_getter_setter_status(status) + } + + 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], + )?; + self.set_joint_impedance([3000., 3000., 3000., 2500., 2500., 2000., 2000.])?; + self.set_cartesian_impedance([3000., 3000., 3000., 300., 300., 300.])?; + Ok(()) + } + + #[allow(non_snake_case)] + fn set_joint_impedance(&mut self, K_theta: [f64; 7]) -> FrankaResult<()> { + let command = ::Data::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); + ::Data::handle_getter_setter_status(status) + } + + #[allow(non_snake_case)] + fn set_cartesian_impedance(&mut self, K_x: [f64; 6]) -> FrankaResult<()> { + let command = ::Data::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); + ::Data::handle_getter_setter_status(status) + } + + #[allow(non_snake_case)] + fn set_load( + &mut self, + load_mass: f64, + F_x_Cload: [f64; 3], + load_inertia: [f64; 9], + ) -> FrankaResult<()> { + let command = ::Data::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); + ::Data::handle_getter_setter_status(status) + } + + fn set_guiding_mode(&mut self, guiding_mode: [bool; 6], elbow: bool) -> FrankaResult<()> { + let command = ::Data::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); + ::Data::handle_getter_setter_status(status) + } + + #[allow(non_snake_case)] + fn set_K(&mut self, EE_T_K: [f64; 16]) -> FrankaResult<()> { + let command = ::Data::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); + ::Data::handle_getter_setter_status(status) + } + + #[allow(non_snake_case)] + fn set_EE(&mut self, NE_T_EE: [f64; 16]) -> FrankaResult<()> { + let command = ::Data::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); + ::Data::handle_getter_setter_status(status) + } + + fn automatic_error_recovery(&mut self) -> FrankaResult<()> { + let command = ::Data::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); + ::Data::handle_automatic_error_recovery_status(status) + } + + fn stop(&mut self) -> FrankaResult<()> { + let command = ::Data::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 control_joint_velocities< + F: FnMut(&RobotState, &Duration) -> JointVelocities, + CM: Into>, + L: Into>, + CF: Into>, + >( + &mut self, + mut motion_generator_callback: F, + controller_mode: CM, + limit_rate: L, + cutoff_frequency: CF, + ) -> FrankaResult<()> { + let cb = |state: &<::Data as RobotData>::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(), + ) + } + + fn control_cartesian_pose< + F: FnMut(&RobotState, &Duration) -> CartesianPose, + CM: Into>, + L: Into>, + CF: Into>, + >( + &mut self, + mut motion_generator_callback: F, + controller_mode: CM, + limit_rate: L, + cutoff_frequency: CF, + ) -> FrankaResult<()> { + let cb = |state: &<::Data as RobotData>::State, duration: &Duration| { + motion_generator_callback(&(state.clone().into()), duration) + }; + ::control_motion_intern( + self, + cb, + controller_mode.into(), + limit_rate.into(), + cutoff_frequency.into(), + ) + } + + fn control_cartesian_velocities< + F: FnMut(&RobotState, &Duration) -> CartesianVelocities, + CM: Into>, + L: Into>, + CF: Into>, + >( + &mut self, + mut motion_generator_callback: F, + controller_mode: CM, + limit_rate: L, + cutoff_frequency: CF, + ) -> FrankaResult<()> { + let cb = |state: &<::Data as RobotData>::State, duration: &Duration| { + motion_generator_callback(&(state.clone().into()), duration) + }; + self.control_motion_intern( + cb, + controller_mode.into(), + limit_rate.into(), + cutoff_frequency.into(), + ) + } + + fn control_torques< + T: FnMut(&RobotState, &Duration) -> Torques, + L: Into>, + CF: Into>, + >( + &mut self, + mut control_callback: T, + limit_rate: L, + cutoff_frequency: CF, + ) -> FrankaResult<()> { + let motion_generator_callback = + |_state: &<::Data as RobotData>::State, _time_step: &Duration| { + JointVelocities::new([0.; 7]) + }; + let mut cb = |state: &<::Data as RobotData>::State, duration: &Duration| { + control_callback(&(state.clone().into()), duration) + }; + self.control_torques_intern( + &motion_generator_callback, + &mut cb, + limit_rate.into(), + cutoff_frequency.into(), + ) + } + + 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, + mut motion_generator_callback: F, + limit_rate: L, + cutoff_frequency: CF, + ) -> FrankaResult<()> { + let motion_generator_cb = |state: &<::Data as RobotData>::State, + duration: &Duration| { + motion_generator_callback(&(state.clone().into()), duration) + }; + let mut control_cb = |state: &<::Data as RobotData>::State, + duration: &Duration| { + control_callback(&(state.clone().into()), duration) + }; + self.control_torques_intern( + motion_generator_cb, + &mut control_cb, + limit_rate.into(), + cutoff_frequency.into(), + ) + } + + fn control_torques_and_joint_positions< + F: FnMut(&RobotState, &Duration) -> JointPositions, + 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_cb = |state: &<::Data as RobotData>::State, + duration: &Duration| { + motion_generator_callback(&(state.clone().into()), duration) + }; + let mut control_cb = |state: &<::Data as RobotData>::State, + duration: &Duration| { + control_callback(&(state.clone().into()), duration) + }; + self.control_torques_intern( + motion_generator_cb, + &mut control_cb, + limit_rate.into(), + cutoff_frequency.into(), + ) + } + + 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, + mut motion_generator_callback: F, + limit_rate: L, + cutoff_frequency: CF, + ) -> FrankaResult<()> { + let motion_generator_cb = |state: &<::Data as RobotData>::State, + duration: &Duration| { + motion_generator_callback(&(state.clone().into()), duration) + }; + let mut control_cb = |state: &<::Data as RobotData>::State, + duration: &Duration| { + control_callback(&(state.clone().into()), duration) + }; + self.control_torques_intern( + motion_generator_cb, + &mut control_cb, + limit_rate.into(), + cutoff_frequency.into(), + ) + } + + 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_cb = |state: &<::Data as RobotData>::State, + duration: &Duration| { + motion_generator_callback(&(state.clone().into()), duration) + }; + let mut control_cb = |state: &<::Data as RobotData>::State, + duration: &Duration| { + control_callback(&(state.clone().into()), duration) + }; + self.control_torques_intern( + motion_generator_cb, + &mut control_cb, + limit_rate.into(), + cutoff_frequency.into(), + ) + } + + fn load_model(&mut self, persistent: bool) -> FrankaResult { + ::get_rob_mut(self).load_model(persistent) + } + + fn server_version(&self) -> u16 { + ::get_rob(self).server_version() + } +} From 59f692000756bec0e929402a030e050546a42415 Mon Sep 17 00:00:00 2001 From: Marco Boneberger Date: Fri, 21 Apr 2023 16:07:41 +0200 Subject: [PATCH 27/63] let clippy fix imports after refactoring --- examples/cartesian_impedance_control.rs | 4 ++-- examples/mirror_robot.rs | 4 ++-- src/gripper.rs | 28 ++++++++++++------------- src/network.rs | 25 +++++++--------------- src/robot.rs | 22 +++++++++---------- src/robot/rate_limiting.rs | 4 ++-- 6 files changed, 38 insertions(+), 49 deletions(-) diff --git a/examples/cartesian_impedance_control.rs b/examples/cartesian_impedance_control.rs index 7ecf272..94b184b 100644 --- a/examples/cartesian_impedance_control.rs +++ b/examples/cartesian_impedance_control.rs @@ -82,8 +82,8 @@ fn generate_motion(mut robot: R) -> 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); diff --git a/examples/mirror_robot.rs b/examples/mirror_robot.rs index 4599ec6..bfdc55e 100644 --- a/examples/mirror_robot.rs +++ b/examples/mirror_robot.rs @@ -139,8 +139,8 @@ fn control_robots( 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); diff --git a/src/gripper.rs b/src/gripper.rs index 13f1478..ba6777f 100644 --- a/src/gripper.rs +++ b/src/gripper.rs @@ -272,7 +272,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); @@ -347,7 +347,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); @@ -356,10 +356,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, @@ -406,7 +406,7 @@ mod tests { .map(|(x, y)| generate_move_request(*x, *y)), )); - let requests_server = requests.clone(); + let requests_server = requests; let thread = std::thread::spawn(|| { let mut mock_gripper = GripperMockServer::new(GRIPPER_VERSION); let mut mock = MockServerReaction::default(); @@ -421,7 +421,7 @@ 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 = GripperResponse { header: GripperCommandHeader::new( @@ -457,7 +457,7 @@ mod tests { let mut mock = MockServerReaction::default(); mock.expect_process_received_bytes() .returning(move |bytes: &mut Vec| -> Vec { - let req: GripperCommandHeader = deserialize(&bytes).unwrap(); + let req: GripperCommandHeader = deserialize(bytes).unwrap(); match req.command { GripperCommandEnum::Stop => {} _ => { @@ -476,7 +476,7 @@ mod tests { 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); }); { @@ -495,7 +495,7 @@ mod tests { let mut mock = MockServerReaction::default(); mock.expect_process_received_bytes() .returning(move |bytes: &mut Vec| -> Vec { - let req: GripperCommandHeader = deserialize(&bytes).unwrap(); + let req: GripperCommandHeader = deserialize(bytes).unwrap(); match req.command { GripperCommandEnum::Homing => {} _ => { @@ -514,7 +514,7 @@ mod tests { 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); }); { @@ -551,7 +551,7 @@ mod tests { |(a, b, c, d, e)| generate_grasp_request(*a, *b, *c, *d, *e), ))); - let requests_server = requests.clone(); + let requests_server = requests; let thread = std::thread::spawn(|| { let mut mock_gripper = GripperMockServer::new(GRIPPER_VERSION); let mut mock = MockServerReaction::default(); @@ -566,7 +566,7 @@ 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 = GripperResponse { header: GripperCommandHeader::new( @@ -609,7 +609,7 @@ mod tests { 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; @@ -638,7 +638,7 @@ mod tests { 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); }); { diff --git a/src/network.rs b/src/network.rs index f5e5de2..005b03e 100644 --- a/src/network.rs +++ b/src/network.rs @@ -26,30 +26,19 @@ use nix::sys::socket::sockopt::{KeepAlive, TcpKeepCount, TcpKeepIdle, TcpKeepInt use serde::de::DeserializeOwned; use serde::Serialize; -use crate::exception::{create_command_exception, FrankaException, FrankaResult}; +use crate::exception::{FrankaException, FrankaResult}; use crate::gripper::types::{ - CommandHeader, GripperCommandEnum, GripperCommandHeader, GRIPPER_VERSION, + CommandHeader, }; use crate::robot::errors::FrankaErrors; use crate::robot::logger::Record; use crate::robot::robot_state::AbstractRobotState; use crate::robot::service_types::{ - AutomaticErrorRecoveryStatusFr3, AutomaticErrorRecoveryStatusPanda, ConnectRequest, - ConnectRequestWithFr3Header, ConnectRequestWithPandaHeader, Fr3CommandEnum, Fr3CommandHeader, - GetterSetterStatusFr3, GetterSetterStatusPanda, LoadModelLibraryRequest, - LoadModelLibraryRequestWithFr3Header, LoadModelLibraryRequestWithPandaHeader, - LoadModelLibraryStatus, MoveRequest, MoveRequestWithFr3Header, MoveRequestWithPandaHeader, - MoveStatusFr3, MoveStatusPanda, PandaCommandEnum, PandaCommandHeader, RobotHeader, - SetCartesianImpedanceRequest, SetCartesianImpedanceRequestWithFr3Header, - SetCartesianImpedanceRequestWithPandaHeader, SetCollisionBehaviorRequest, - SetCollisionBehaviorRequestWithFr3Header, SetCollisionBehaviorRequestWithPandaHeader, - SetEeToKRequest, SetEeToKRequestWithFr3Header, SetEeToKRequestWithPandaHeader, - SetGuidingModeRequest, SetGuidingModeRequestWithFr3Header, - SetGuidingModeRequestWithPandaHeader, SetJointImpedanceRequest, - SetJointImpedanceRequestWithFr3Header, SetJointImpedanceRequestWithPandaHeader, SetLoadRequest, - SetLoadRequestWithFr3Header, SetLoadRequestWithPandaHeader, SetNeToEeRequest, - SetNeToEeRequestWithFr3Header, SetNeToEeRequestWithPandaHeader, StopMoveStatusFr3, - StopMoveStatusPanda, FR3_VERSION, PANDA_VERSION, + ConnectRequest, LoadModelLibraryRequest, + LoadModelLibraryStatus, MoveRequest, PandaCommandEnum, PandaCommandHeader, RobotHeader, + SetCartesianImpedanceRequest, SetCollisionBehaviorRequest, + SetEeToKRequest, + SetGuidingModeRequest, SetJointImpedanceRequest, SetLoadRequest, SetNeToEeRequest, }; use crate::robot::types::AbstractRobotStateIntern; use crate::{RobotModel, RobotState}; diff --git a/src/robot.rs b/src/robot.rs index b851d0e..ad23a3b 100644 --- a/src/robot.rs +++ b/src/robot.rs @@ -99,7 +99,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); @@ -169,7 +169,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); @@ -178,10 +178,10 @@ mod tests { &self, tcp_socket: &mut Socket, ) -> ConnectRequestWithPandaHeader { - let mut bytes = vec![0 as u8; 100]; + let mut bytes = vec![0_u8; 100]; (tcp_socket.receive_bytes)(&mut bytes); let request: ConnectRequestWithPandaHeader = deserialize(bytes.as_slice()).unwrap(); - return request; + request } fn send_robot_connect_response), G: Fn(&mut Vec)>( &self, @@ -255,7 +255,7 @@ 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(FR3_VERSION); let mut mock = MockServerReaction::default(); @@ -271,7 +271,7 @@ mod tests { .zip(serialized_expected_request.iter()) .for_each(|(x, y)| assert_eq!(x, y)); let req: SetCollisionBehaviorRequestWithPandaHeader = - deserialize(&bytes).unwrap(); + deserialize(bytes).unwrap(); counter += 1; let mut response = SetterResponseFr3 { header: Fr3CommandHeader::new( @@ -326,7 +326,7 @@ mod tests { }, ), }]); - let requests_server = requests.clone(); + let requests_server = requests; let thread = std::thread::spawn(|| { let mut robot_server = RobotMockServer::new(FR3_VERSION); let mut mock = MockServerReaction::default(); @@ -336,7 +336,7 @@ 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: MoveRequestWithPandaHeader = deserialize(&bytes).unwrap(); + let req: MoveRequestWithPandaHeader = deserialize(bytes).unwrap(); assert_eq!(bytes.len(), serialized_expected_request.len()); bytes .iter() @@ -390,7 +390,7 @@ mod tests { 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)); @@ -417,7 +417,7 @@ mod tests { 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); }); @@ -437,7 +437,7 @@ mod tests { 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); }); { diff --git a/src/robot/rate_limiting.rs b/src/robot/rate_limiting.rs index e924413..10594bf 100644 --- a/src/robot/rate_limiting.rs +++ b/src/robot/rate_limiting.rs @@ -504,7 +504,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.), @@ -527,7 +527,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 From ad26582f4810e9eee591a0361e0cafc77f667aed Mon Sep 17 00:00:00 2001 From: Marco Boneberger Date: Fri, 21 Apr 2023 16:18:06 +0200 Subject: [PATCH 28/63] extract device_data.rs and robot_data.rs --- src/device_data.rs | 12 +++ src/gripper.rs | 3 +- src/lib.rs | 4 +- src/model/library_downloader.rs | 3 +- src/network.rs | 162 +------------------------------- src/robot/control_loop.rs | 2 +- src/robot/fr3.rs | 6 +- src/robot/panda.rs | 6 +- src/robot/robot.rs | 3 +- src/robot/robot_impl.rs | 3 +- src/robot/robot_wrapper.rs | 4 +- src/robot_data.rs | 147 +++++++++++++++++++++++++++++ 12 files changed, 186 insertions(+), 169 deletions(-) create mode 100644 src/device_data.rs create mode 100644 src/robot_data.rs diff --git a/src/device_data.rs b/src/device_data.rs new file mode 100644 index 0000000..e10becb --- /dev/null +++ b/src/device_data.rs @@ -0,0 +1,12 @@ +use crate::gripper::types::CommandHeader; + +pub 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/gripper.rs b/src/gripper.rs index ba6777f..af64d75 100644 --- a/src/gripper.rs +++ b/src/gripper.rs @@ -3,6 +3,7 @@ //! Contains the franka::Gripper type. +use crate::device_data::DeviceData; use std::mem::size_of; use crate::exception::{create_command_exception, FrankaException, FrankaResult}; @@ -12,7 +13,7 @@ use crate::gripper::types::{ GraspRequestWithHeader, GripperCommandEnum, GripperCommandHeader, GripperStateIntern, MoveRequest, MoveRequestWithHeader, Status, COMMAND_PORT, GRIPPER_VERSION, }; -use crate::network::{DeviceData, Network}; +use crate::network::Network; pub mod gripper_state; pub(crate) mod types; diff --git a/src/lib.rs b/src/lib.rs index 470ccf5..1f326ca 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -168,7 +168,9 @@ pub mod gripper; mod network; pub mod robot; +mod device_data; pub mod model; +mod robot_data; pub mod utils; pub use exception::FrankaResult; @@ -178,7 +180,6 @@ pub use model::Fr3Model; pub use model::Frame; pub use model::PandaModel; pub use model::RobotModel; -pub use network::RobotData; pub use robot::control_types::*; pub use robot::fr3::Fr3; pub use robot::low_pass_filter::DEFAULT_CUTOFF_FREQUENCY; @@ -186,4 +187,5 @@ pub use robot::low_pass_filter::MAX_CUTOFF_FREQUENCY; pub use robot::panda::Panda; pub use robot::robot_state::RobotState; pub use robot::robot_wrapper::RobotWrapper; +pub use robot_data::RobotData; pub use utils::*; diff --git a/src/model/library_downloader.rs b/src/model/library_downloader.rs index 9f766b3..e4e7809 100644 --- a/src/model/library_downloader.rs +++ b/src/model/library_downloader.rs @@ -1,11 +1,12 @@ // Copyright (c) 2021 Marco Boneberger // Licensed under the EUPL-1.2-or-later use crate::exception::FrankaException::ModelException; -use crate::network::{Network, RobotData}; +use crate::network::Network; use crate::robot::service_types::{ LoadModelLibraryArchitecture, LoadModelLibraryRequest, LoadModelLibraryStatus, LoadModelLibrarySystem, }; +use crate::robot_data::RobotData; use crate::FrankaResult; use std::fmt; use std::fmt::Display; diff --git a/src/network.rs b/src/network.rs index 005b03e..f93c423 100644 --- a/src/network.rs +++ b/src/network.rs @@ -27,21 +27,11 @@ use serde::de::DeserializeOwned; use serde::Serialize; use crate::exception::{FrankaException, FrankaResult}; -use crate::gripper::types::{ - CommandHeader, -}; -use crate::robot::errors::FrankaErrors; -use crate::robot::logger::Record; -use crate::robot::robot_state::AbstractRobotState; -use crate::robot::service_types::{ - ConnectRequest, LoadModelLibraryRequest, - LoadModelLibraryStatus, MoveRequest, PandaCommandEnum, PandaCommandHeader, RobotHeader, - SetCartesianImpedanceRequest, SetCollisionBehaviorRequest, - SetEeToKRequest, - SetGuidingModeRequest, SetJointImpedanceRequest, SetLoadRequest, SetNeToEeRequest, -}; -use crate::robot::types::AbstractRobotStateIntern; -use crate::{RobotModel, RobotState}; +use crate::gripper::types::CommandHeader; + +use crate::robot::service_types::{LoadModelLibraryStatus, PandaCommandEnum, PandaCommandHeader}; + +use crate::device_data::DeviceData; const CLIENT: Token = Token(1); @@ -51,148 +41,6 @@ pub enum NetworkType { Gripper, } -pub 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; -} - -pub trait RobotData: DeviceData { - type DeviceData: DeviceData; - type Header: RobotHeader; - type State: AbstractRobotState + From + From; - type StateIntern: Debug + DeserializeOwned + Serialize + AbstractRobotStateIntern + 'static; - type Model: RobotModel; - 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>; -} - pub trait MessageCommand { fn get_command_message_id(&self) -> u32; } diff --git a/src/robot/control_loop.rs b/src/robot/control_loop.rs index 58f21e9..b466444 100644 --- a/src/robot/control_loop.rs +++ b/src/robot/control_loop.rs @@ -4,7 +4,6 @@ use std::fmt::Debug; use std::time::Duration; use crate::exception::{FrankaException, FrankaResult}; -use crate::network::RobotData; use crate::robot::control_tools::{ has_realtime_kernel, set_current_thread_to_highest_scheduler_priority, }; @@ -16,6 +15,7 @@ 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::robot_data::RobotData; use crate::Finishable; type ControlCallback<'b, State> = &'b mut dyn FnMut(&State, &Duration) -> Torques; diff --git a/src/robot/fr3.rs b/src/robot/fr3.rs index a610ab6..64c0e0c 100644 --- a/src/robot/fr3.rs +++ b/src/robot/fr3.rs @@ -1,5 +1,6 @@ +use crate::device_data::DeviceData; use crate::exception::{create_command_exception, FrankaException}; -use crate::network::{DeviceData, Network}; +use crate::network::Network; use crate::robot::errors::FrankaErrors; use crate::robot::logger::Record; use crate::robot::robot::{PrivateRobot, Robot}; @@ -14,7 +15,8 @@ use crate::robot::service_types::{ SetLoadRequestWithFr3Header, SetNeToEeRequestWithFr3Header, StopMoveStatusFr3, FR3_VERSION, }; use crate::robot::types::Fr3StateIntern; -use crate::{Fr3Model, FrankaResult, RealtimeConfig, RobotData, RobotState}; +use crate::robot_data::RobotData; +use crate::{Fr3Model, FrankaResult, RealtimeConfig, RobotState}; use std::mem::size_of; pub struct Fr3 { diff --git a/src/robot/panda.rs b/src/robot/panda.rs index 9d5aae5..ca0e8d0 100644 --- a/src/robot/panda.rs +++ b/src/robot/panda.rs @@ -1,5 +1,6 @@ +use crate::device_data::DeviceData; use crate::exception::{create_command_exception, FrankaException}; -use crate::network::{DeviceData, Network}; +use crate::network::Network; use crate::robot::errors::FrankaErrors; use crate::robot::logger::Record; use crate::robot::robot::{PrivateRobot, Robot}; @@ -18,7 +19,8 @@ use crate::robot::service_types::{ }; use crate::robot::types::PandaStateIntern; use crate::robot::virtual_wall_cuboid::VirtualWallCuboid; -use crate::{FrankaResult, PandaModel, RealtimeConfig, RobotData, RobotState}; +use crate::robot_data::RobotData; +use crate::{FrankaResult, PandaModel, RealtimeConfig, RobotState}; use std::mem::size_of; /// Maintains a network connection to the robot, provides the current robot state, gives access to diff --git a/src/robot/robot.rs b/src/robot/robot.rs index 55f7a0a..4e92ada 100644 --- a/src/robot/robot.rs +++ b/src/robot/robot.rs @@ -2,9 +2,10 @@ use crate::network::Network; use crate::robot::control_loop::ControlLoop; use crate::robot::motion_generator_traits::MotionGeneratorTrait; use crate::robot::robot_impl::RobotImplementation; +use crate::robot_data::RobotData; use crate::{ CartesianPose, CartesianVelocities, ControllerMode, ConvertMotion, Finishable, FrankaResult, - JointPositions, JointVelocities, RobotData, RobotState, Torques, DEFAULT_CUTOFF_FREQUENCY, + JointPositions, JointVelocities, RobotState, Torques, DEFAULT_CUTOFF_FREQUENCY, }; use std::fmt::Debug; use std::time::Duration; diff --git a/src/robot/robot_impl.rs b/src/robot/robot_impl.rs index b9e5ce5..23250d6 100644 --- a/src/robot/robot_impl.rs +++ b/src/robot/robot_impl.rs @@ -2,7 +2,7 @@ // Licensed under the EUPL-1.2-or-later use crate::exception::{FrankaException, FrankaResult}; use crate::model::library_downloader::{LibraryDownloader, LibraryDownloaderGeneric}; -use crate::network::{Network, RobotData}; +use crate::network::Network; use crate::robot::control_types::RealtimeConfig; use crate::robot::logger::Logger; @@ -16,6 +16,7 @@ use crate::robot::types::{ AbstractRobotStateIntern, ControllerCommand, ControllerMode, MotionGeneratorCommand, MotionGeneratorMode, RobotCommand, }; +use crate::robot_data::RobotData; use crate::RobotModel; use std::fs::remove_file; use std::path::Path; diff --git a/src/robot/robot_wrapper.rs b/src/robot/robot_wrapper.rs index b6d121f..a9adbf8 100644 --- a/src/robot/robot_wrapper.rs +++ b/src/robot/robot_wrapper.rs @@ -8,10 +8,10 @@ use crate::robot::service_types::{ SetGuidingModeRequest, SetJointImpedanceRequest, SetLoadRequest, SetNeToEeRequest, StopMoveStatusPanda, }; +use crate::robot_data::RobotData; use crate::{ CartesianPose, CartesianVelocities, ControllerMode, FrankaResult, JointPositions, - JointVelocities, MotionGenerator, RobotData, RobotModel, RobotState, Torques, - MAX_CUTOFF_FREQUENCY, + JointVelocities, MotionGenerator, RobotModel, RobotState, Torques, MAX_CUTOFF_FREQUENCY, }; use std::time::Duration; diff --git a/src/robot_data.rs b/src/robot_data.rs new file mode 100644 index 0000000..81fab95 --- /dev/null +++ b/src/robot_data.rs @@ -0,0 +1,147 @@ +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::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, RobotModel, RobotState}; +use serde::de::DeserializeOwned; +use serde::Serialize; +use std::fmt::Debug; + +pub trait RobotData: DeviceData { + type DeviceData: DeviceData; + type Header: RobotHeader; + type State: AbstractRobotState + From + From; + type StateIntern: Debug + DeserializeOwned + Serialize + AbstractRobotStateIntern + 'static; + type Model: RobotModel; + 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>; +} From 90b00f127e4772689ee596b460d030073482becb Mon Sep 17 00:00:00 2001 From: Marco Boneberger Date: Fri, 21 Apr 2023 16:20:31 +0200 Subject: [PATCH 29/63] move robot_data.rs to robot folder --- src/lib.rs | 3 +-- src/model/library_downloader.rs | 2 +- src/robot.rs | 1 + src/robot/control_loop.rs | 2 +- src/robot/fr3.rs | 2 +- src/robot/panda.rs | 2 +- src/robot/robot.rs | 2 +- src/{ => robot}/robot_data.rs | 0 src/robot/robot_impl.rs | 2 +- src/robot/robot_wrapper.rs | 2 +- 10 files changed, 9 insertions(+), 9 deletions(-) rename src/{ => robot}/robot_data.rs (100%) diff --git a/src/lib.rs b/src/lib.rs index 1f326ca..580c527 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -170,7 +170,6 @@ pub mod robot; mod device_data; pub mod model; -mod robot_data; pub mod utils; pub use exception::FrankaResult; @@ -185,7 +184,7 @@ pub use robot::fr3::Fr3; pub use robot::low_pass_filter::DEFAULT_CUTOFF_FREQUENCY; pub use robot::low_pass_filter::MAX_CUTOFF_FREQUENCY; pub use robot::panda::Panda; +pub use robot::robot_data::RobotData; pub use robot::robot_state::RobotState; pub use robot::robot_wrapper::RobotWrapper; -pub use robot_data::RobotData; pub use utils::*; diff --git a/src/model/library_downloader.rs b/src/model/library_downloader.rs index e4e7809..5218ef2 100644 --- a/src/model/library_downloader.rs +++ b/src/model/library_downloader.rs @@ -2,11 +2,11 @@ // Licensed under the EUPL-1.2-or-later use crate::exception::FrankaException::ModelException; use crate::network::Network; +use crate::robot::robot_data::RobotData; use crate::robot::service_types::{ LoadModelLibraryArchitecture, LoadModelLibraryRequest, LoadModelLibraryStatus, LoadModelLibrarySystem, }; -use crate::robot_data::RobotData; use crate::FrankaResult; use std::fmt; use std::fmt::Display; diff --git a/src/robot.rs b/src/robot.rs index ad23a3b..ef9f056 100644 --- a/src/robot.rs +++ b/src/robot.rs @@ -16,6 +16,7 @@ pub mod panda; mod rate_limiting; pub mod robot; mod robot_control; +pub mod robot_data; mod robot_impl; pub mod robot_state; pub mod robot_wrapper; diff --git a/src/robot/control_loop.rs b/src/robot/control_loop.rs index b466444..44788f4 100644 --- a/src/robot/control_loop.rs +++ b/src/robot/control_loop.rs @@ -11,11 +11,11 @@ use crate::robot::control_types::{ControllerMode, ConvertMotion, RealtimeConfig, 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_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::robot_data::RobotData; use crate::Finishable; type ControlCallback<'b, State> = &'b mut dyn FnMut(&State, &Duration) -> Torques; diff --git a/src/robot/fr3.rs b/src/robot/fr3.rs index 64c0e0c..60145dd 100644 --- a/src/robot/fr3.rs +++ b/src/robot/fr3.rs @@ -4,6 +4,7 @@ use crate::network::Network; use crate::robot::errors::FrankaErrors; use crate::robot::logger::Record; use crate::robot::robot::{PrivateRobot, Robot}; +use crate::robot::robot_data::RobotData; use crate::robot::robot_impl::RobotImplGeneric; use crate::robot::service_types; use crate::robot::service_types::{ @@ -15,7 +16,6 @@ use crate::robot::service_types::{ SetLoadRequestWithFr3Header, SetNeToEeRequestWithFr3Header, StopMoveStatusFr3, FR3_VERSION, }; use crate::robot::types::Fr3StateIntern; -use crate::robot_data::RobotData; use crate::{Fr3Model, FrankaResult, RealtimeConfig, RobotState}; use std::mem::size_of; diff --git a/src/robot/panda.rs b/src/robot/panda.rs index ca0e8d0..65f6090 100644 --- a/src/robot/panda.rs +++ b/src/robot/panda.rs @@ -4,6 +4,7 @@ use crate::network::Network; use crate::robot::errors::FrankaErrors; use crate::robot::logger::Record; use crate::robot::robot::{PrivateRobot, Robot}; +use crate::robot::robot_data::RobotData; use crate::robot::robot_impl::RobotImplGeneric; use crate::robot::service_types; use crate::robot::service_types::{ @@ -19,7 +20,6 @@ use crate::robot::service_types::{ }; use crate::robot::types::PandaStateIntern; use crate::robot::virtual_wall_cuboid::VirtualWallCuboid; -use crate::robot_data::RobotData; use crate::{FrankaResult, PandaModel, RealtimeConfig, RobotState}; use std::mem::size_of; diff --git a/src/robot/robot.rs b/src/robot/robot.rs index 4e92ada..4697a53 100644 --- a/src/robot/robot.rs +++ b/src/robot/robot.rs @@ -1,8 +1,8 @@ use crate::network::Network; use crate::robot::control_loop::ControlLoop; use crate::robot::motion_generator_traits::MotionGeneratorTrait; +use crate::robot::robot_data::RobotData; use crate::robot::robot_impl::RobotImplementation; -use crate::robot_data::RobotData; use crate::{ CartesianPose, CartesianVelocities, ControllerMode, ConvertMotion, Finishable, FrankaResult, JointPositions, JointVelocities, RobotState, Torques, DEFAULT_CUTOFF_FREQUENCY, diff --git a/src/robot_data.rs b/src/robot/robot_data.rs similarity index 100% rename from src/robot_data.rs rename to src/robot/robot_data.rs diff --git a/src/robot/robot_impl.rs b/src/robot/robot_impl.rs index 23250d6..76cbaf3 100644 --- a/src/robot/robot_impl.rs +++ b/src/robot/robot_impl.rs @@ -7,6 +7,7 @@ use crate::robot::control_types::RealtimeConfig; use crate::robot::logger::Logger; use crate::robot::robot_control::RobotControl; +use crate::robot::robot_data::RobotData; use crate::robot::robot_state::AbstractRobotState; use crate::robot::service_types::{ ConnectResponseWithoutHeader, ConnectStatus, MoveControllerMode, MoveDeviation, @@ -16,7 +17,6 @@ use crate::robot::types::{ AbstractRobotStateIntern, ControllerCommand, ControllerMode, MotionGeneratorCommand, MotionGeneratorMode, RobotCommand, }; -use crate::robot_data::RobotData; use crate::RobotModel; use std::fs::remove_file; use std::path::Path; diff --git a/src/robot/robot_wrapper.rs b/src/robot/robot_wrapper.rs index a9adbf8..f68d284 100644 --- a/src/robot/robot_wrapper.rs +++ b/src/robot/robot_wrapper.rs @@ -2,13 +2,13 @@ use crate::exception::create_command_exception; use crate::robot::robot::PrivateRobot; use crate::robot::robot::Robot; use crate::robot::robot_control::RobotControl; +use crate::robot::robot_data::RobotData; use crate::robot::robot_impl::RobotImplementation; use crate::robot::service_types::{ SetCartesianImpedanceRequest, SetCollisionBehaviorRequest, SetEeToKRequest, SetGuidingModeRequest, SetJointImpedanceRequest, SetLoadRequest, SetNeToEeRequest, StopMoveStatusPanda, }; -use crate::robot_data::RobotData; use crate::{ CartesianPose, CartesianVelocities, ControllerMode, FrankaResult, JointPositions, JointVelocities, MotionGenerator, RobotModel, RobotState, Torques, MAX_CUTOFF_FREQUENCY, From 0817f780ffb4c413f7eaaec486b785e4107a51a0 Mon Sep 17 00:00:00 2001 From: Marco Boneberger Date: Fri, 21 Apr 2023 16:42:18 +0200 Subject: [PATCH 30/63] rename robot.rs to robot_trait.rs to fix clippy warning --- src/robot.rs | 2 +- src/robot/fr3.rs | 2 +- src/robot/panda.rs | 2 +- src/robot/{robot.rs => robot_trait.rs} | 0 src/robot/robot_wrapper.rs | 4 ++-- 5 files changed, 5 insertions(+), 5 deletions(-) rename src/robot/{robot.rs => robot_trait.rs} (100%) diff --git a/src/robot.rs b/src/robot.rs index ef9f056..45180d2 100644 --- a/src/robot.rs +++ b/src/robot.rs @@ -14,11 +14,11 @@ pub mod low_pass_filter; mod motion_generator_traits; pub mod panda; mod rate_limiting; -pub mod robot; mod robot_control; pub mod robot_data; mod robot_impl; pub mod robot_state; +pub mod robot_trait; pub mod robot_wrapper; pub(crate) mod service_types; pub(crate) mod types; diff --git a/src/robot/fr3.rs b/src/robot/fr3.rs index 60145dd..2edcc0d 100644 --- a/src/robot/fr3.rs +++ b/src/robot/fr3.rs @@ -3,9 +3,9 @@ use crate::exception::{create_command_exception, FrankaException}; use crate::network::Network; use crate::robot::errors::FrankaErrors; use crate::robot::logger::Record; -use crate::robot::robot::{PrivateRobot, Robot}; use crate::robot::robot_data::RobotData; use crate::robot::robot_impl::RobotImplGeneric; +use crate::robot::robot_trait::{PrivateRobot, Robot}; use crate::robot::service_types; use crate::robot::service_types::{ AutomaticErrorRecoveryStatusFr3, ConnectRequest, ConnectRequestWithFr3Header, Fr3CommandEnum, diff --git a/src/robot/panda.rs b/src/robot/panda.rs index 65f6090..a7e69bb 100644 --- a/src/robot/panda.rs +++ b/src/robot/panda.rs @@ -3,9 +3,9 @@ use crate::exception::{create_command_exception, FrankaException}; use crate::network::Network; use crate::robot::errors::FrankaErrors; use crate::robot::logger::Record; -use crate::robot::robot::{PrivateRobot, Robot}; use crate::robot::robot_data::RobotData; use crate::robot::robot_impl::RobotImplGeneric; +use crate::robot::robot_trait::{PrivateRobot, Robot}; use crate::robot::service_types; use crate::robot::service_types::{ AutomaticErrorRecoveryStatusPanda, ConnectRequest, ConnectRequestWithPandaHeader, diff --git a/src/robot/robot.rs b/src/robot/robot_trait.rs similarity index 100% rename from src/robot/robot.rs rename to src/robot/robot_trait.rs diff --git a/src/robot/robot_wrapper.rs b/src/robot/robot_wrapper.rs index f68d284..62cfd26 100644 --- a/src/robot/robot_wrapper.rs +++ b/src/robot/robot_wrapper.rs @@ -1,9 +1,9 @@ use crate::exception::create_command_exception; -use crate::robot::robot::PrivateRobot; -use crate::robot::robot::Robot; use crate::robot::robot_control::RobotControl; use crate::robot::robot_data::RobotData; use crate::robot::robot_impl::RobotImplementation; +use crate::robot::robot_trait::PrivateRobot; +use crate::robot::robot_trait::Robot; use crate::robot::service_types::{ SetCartesianImpedanceRequest, SetCollisionBehaviorRequest, SetEeToKRequest, SetGuidingModeRequest, SetJointImpedanceRequest, SetLoadRequest, SetNeToEeRequest, From 746f9585c5bed9156d9e05398904999c89c94854 Mon Sep 17 00:00:00 2001 From: Marco Boneberger Date: Fri, 28 Apr 2023 16:57:26 +0200 Subject: [PATCH 31/63] fix gripper tests --- src/gripper.rs | 68 +++++++++++++++++++++++--------------------------- 1 file changed, 31 insertions(+), 37 deletions(-) diff --git a/src/gripper.rs b/src/gripper.rs index af64d75..7e6be2a 100644 --- a/src/gripper.rs +++ b/src/gripper.rs @@ -233,6 +233,7 @@ mod tests { struct GripperMockServer { server_version: u16, + srv_sock: TcpListener, } pub struct ServerReaction {} @@ -249,19 +250,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)); @@ -284,17 +292,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() @@ -317,7 +317,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, @@ -408,8 +408,8 @@ mod tests { )); let requests_server = requests; - let thread = std::thread::spawn(|| { - let mut mock_gripper = GripperMockServer::new(GRIPPER_VERSION); + 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; @@ -440,7 +440,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"); assert_eq!(gripper.server_version(), GRIPPER_VERSION); for (width, speed) in move_request_values.iter() { @@ -453,8 +452,8 @@ mod tests { #[test] fn gripper_stop_test() -> FrankaResult<()> { - let thread = std::thread::spawn(|| { - let mut mock_gripper = GripperMockServer::new(GRIPPER_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 { @@ -481,7 +480,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"); gripper.stop().unwrap(); } @@ -491,8 +489,8 @@ mod tests { #[test] fn gripper_homing_test() -> FrankaResult<()> { - let thread = std::thread::spawn(|| { - let mut mock_gripper = GripperMockServer::new(GRIPPER_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 { @@ -519,7 +517,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"); gripper.homing().unwrap(); } @@ -553,8 +550,8 @@ mod tests { ))); let requests_server = requests; - let thread = std::thread::spawn(|| { - let mut mock_gripper = GripperMockServer::new(GRIPPER_VERSION); + 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; @@ -585,7 +582,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 @@ -605,8 +601,8 @@ mod tests { #[test] fn incompatible_library() { - let thread = std::thread::spawn(|| { - let mut mock_gripper = GripperMockServer::new(GRIPPER_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()); @@ -615,7 +611,6 @@ mod tests { }); let gripper_result; { - std::thread::sleep(Duration::from_secs_f64(0.01)); gripper_result = Gripper::new("127.0.0.1") } thread.join().unwrap(); @@ -634,8 +629,8 @@ mod tests { #[test] fn gripper_read_once() { - let thread = std::thread::spawn(|| { - let mut server = GripperMockServer::new(GRIPPER_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()); @@ -643,7 +638,6 @@ mod tests { 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"); } From 715d51516a0b40af1661dd9d2bc1fc0d51053ab0 Mon Sep 17 00:00:00 2001 From: Marco Boneberger Date: Fri, 5 May 2023 13:21:54 +0200 Subject: [PATCH 32/63] update documentation of model Module and avoid copy pasting code for PandaModel --- src/model.rs | 408 +++++--------------------------- src/model/library_downloader.rs | 32 +-- src/model/model_library.rs | 2 +- 3 files changed, 65 insertions(+), 377 deletions(-) diff --git a/src/model.rs b/src/model.rs index 488abfc..11f2f66 100644 --- a/src/model.rs +++ b/src/model.rs @@ -1,7 +1,13 @@ // 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. +//! +//! [Fr3Model] is the Model for the FR3 and [PandaModel] is the Model for the Panda. +//! They are wrappers around a shared object that is received from the robot. +//! The shared object contains methods to calculate jacobians, mass matrix etc. for the robot. +//! Both [Fr3Model] and [PandaModel] implement the [RobotModel] trait that +//! contains the methods to get the needed info from the Model. use nalgebra::Matrix4; use crate::model::model_library::ModelLibrary; @@ -64,85 +70,13 @@ impl fmt::Display for Frame { } } +/// Provides jacobians, mass matrix, coriolis torques, gravity torques and forward kinematics of +/// a robot. pub trait RobotModel { - fn new>(model_filename: S, libm_filename: Option<&Path>) -> FrankaResult - where - Self: Sized; - #[allow(non_snake_case)] - fn pose( - &self, - frame: &Frame, - q: &[f64; 7], - F_T_EE: &[f64; 16], - EE_T_K: &[f64; 16], - ) -> [f64; 16]; - - fn pose_from_state(&self, frame: &Frame, robot_state: &RobotState) -> [f64; 16]; - #[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]; - - fn body_jacobian_from_state(&self, frame: &Frame, robot_state: &RobotState) -> [f64; 42]; - #[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]; - fn zero_jacobian_from_state(&self, frame: &Frame, robot_state: &RobotState) -> [f64; 42]; - #[allow(non_snake_case)] - fn mass( - &self, - q: &[f64; 7], - I_total: &[f64; 9], - m_total: f64, - F_x_Ctotal: &[f64; 3], - ) -> [f64; 49]; - fn mass_from_state(&self, robot_state: &RobotState) -> [f64; 49]; - #[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]; - fn coriolis_from_state(&self, robot_state: &RobotState) -> [f64; 7]; - #[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]; - #[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 the robot. -pub struct PandaModel { - library: ModelLibrary, -} - -#[allow(non_snake_case)] -impl RobotModel for PandaModel { /// 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`). + /// [`RobotWrapper::load_model`](crate::RobotWrapper::load_model). /// /// If you do not have the model you can use the download_model example to download the model /// # Arguments @@ -167,11 +101,9 @@ impl RobotModel for PandaModel { /// 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 \ ¯\_(ツ)_/¯ - fn new>(model_filename: S, libm_filename: Option<&Path>) -> FrankaResult { - Ok(PandaModel { - library: ModelLibrary::new(model_filename.as_ref(), libm_filename)?, - }) - } + fn new>(model_filename: S, libm_filename: Option<&Path>) -> FrankaResult + where + Self: Sized; /// Gets the 4x4 pose matrix for the given frame in base frame. /// /// The pose is represented as a 4x4 matrix in column-major format. @@ -182,36 +114,15 @@ impl RobotModel for PandaModel { /// * `EE_T_K` - Stiffness frame K in the end effector frame. /// # Return /// Vectorized 4x4 pose matrix, column-major. + #[allow(non_snake_case)] fn pose( &self, frame: &Frame, q: &[f64; 7], F_T_EE: &[f64; 16], EE_T_K: &[f64; 16], - ) -> [f64; 16] { - let mut output = [0.; 16]; - match frame { - Frame::Joint1 => self.library.joint1(q, &mut output), - Frame::Joint2 => self.library.joint2(q, &mut output), - Frame::Joint3 => self.library.joint3(q, &mut output), - Frame::Joint4 => self.library.joint4(q, &mut output), - Frame::Joint5 => self.library.joint5(q, &mut output), - Frame::Joint6 => self.library.joint6(q, &mut output), - Frame::Joint7 => self.library.joint7(q, &mut output), - Frame::Flange => self.library.flange(q, &mut output), - Frame::EndEffector => self.library.ee(q, F_T_EE, &mut output), - Frame::Stiffness => { - let tmp: Matrix4 = - Matrix4::from_column_slice(F_T_EE) * Matrix4::from_column_slice(EE_T_K); - let mut stiffness_f_t_ee = [0.; 16]; - tmp.iter() - .enumerate() - .for_each(|(i, &x)| stiffness_f_t_ee[i] = x); - self.library.ee(q, &stiffness_f_t_ee, &mut output) - } - } - output - } + ) -> [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. @@ -220,14 +131,8 @@ impl RobotModel for PandaModel { /// * `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] { - self.pose( - frame, - &robot_state.q, - &robot_state.F_T_EE, - &robot_state.EE_T_K, - ) - } + 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. @@ -238,37 +143,14 @@ impl RobotModel for PandaModel { /// * `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] { - let mut output = [0.; 42]; - match frame { - Frame::Joint1 => self.library.body_jacobian_joint1(&mut output), - Frame::Joint2 => self.library.body_jacobian_joint2(q, &mut output), - Frame::Joint3 => self.library.body_jacobian_joint3(q, &mut output), - Frame::Joint4 => self.library.body_jacobian_joint4(q, &mut output), - Frame::Joint5 => self.library.body_jacobian_joint5(q, &mut output), - Frame::Joint6 => self.library.body_jacobian_joint6(q, &mut output), - Frame::Joint7 => self.library.body_jacobian_joint7(q, &mut output), - Frame::Flange => self.library.body_jacobian_flange(q, &mut output), - Frame::EndEffector => self.library.body_jacobian_ee(q, F_T_EE, &mut output), - Frame::Stiffness => { - let tmp: Matrix4 = - Matrix4::from_column_slice(F_T_EE) * Matrix4::from_column_slice(EE_T_K); - let mut stiffness_f_t_ee = [0.; 16]; - tmp.iter() - .enumerate() - .for_each(|(i, &x)| stiffness_f_t_ee[i] = x); - self.library - .body_jacobian_ee(q, &stiffness_f_t_ee, &mut output) - } - } - output - } + ) -> [f64; 42]; /// Gets the 6x7 Jacobian for the given frame, relative to that frame. /// @@ -278,14 +160,8 @@ impl RobotModel for PandaModel { /// * `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] { - self.body_jacobian( - frame, - &robot_state.q, - &robot_state.F_T_EE, - &robot_state.EE_T_K, - ) - } + 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. @@ -296,37 +172,15 @@ impl RobotModel for PandaModel { /// * `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] { - let mut output = [0.; 42]; - match frame { - Frame::Joint1 => self.library.zero_jacobian_joint1(&mut output), - Frame::Joint2 => self.library.zero_jacobian_joint2(q, &mut output), - Frame::Joint3 => self.library.zero_jacobian_joint3(q, &mut output), - Frame::Joint4 => self.library.zero_jacobian_joint4(q, &mut output), - Frame::Joint5 => self.library.zero_jacobian_joint5(q, &mut output), - Frame::Joint6 => self.library.zero_jacobian_joint6(q, &mut output), - Frame::Joint7 => self.library.zero_jacobian_joint7(q, &mut output), - Frame::Flange => self.library.zero_jacobian_flange(q, &mut output), - Frame::EndEffector => self.library.zero_jacobian_ee(q, F_T_EE, &mut output), - Frame::Stiffness => { - let tmp: Matrix4 = - Matrix4::from_column_slice(F_T_EE) * Matrix4::from_column_slice(EE_T_K); - let mut stiffness_f_t_ee = [0.; 16]; - tmp.iter() - .enumerate() - .for_each(|(i, &x)| stiffness_f_t_ee[i] = x); - self.library - .zero_jacobian_ee(q, &stiffness_f_t_ee, &mut output) - } - } - output - } + ) -> [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. @@ -335,14 +189,8 @@ impl RobotModel for PandaModel { /// * `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] { - self.zero_jacobian( - frame, - &robot_state.q, - &robot_state.F_T_EE, - &robot_state.EE_T_K, - ) - } + 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. @@ -352,31 +200,22 @@ impl RobotModel for PandaModel { /// * `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] { - let mut output = [0.; 49]; - self.library - .mass(q, I_total, &m_total, F_x_Ctotal, &mut output); - output - } + ) -> [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] { - self.mass( - &robot_state.q, - &robot_state.I_total, - robot_state.m_total, - &robot_state.F_x_Ctotal, - ) - } + 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 @@ -388,6 +227,7 @@ impl RobotModel for PandaModel { /// * `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], @@ -395,12 +235,7 @@ impl RobotModel for PandaModel { I_total: &[f64; 9], m_total: f64, F_x_Ctotal: &[f64; 3], - ) -> [f64; 7] { - let mut output = [0.; 7]; - self.library - .coriolis(q, dq, I_total, &m_total, F_x_Ctotal, &mut output); - output - } + ) -> [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\]. @@ -408,15 +243,8 @@ impl RobotModel for PandaModel { /// * `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] { - self.coriolis( - &robot_state.q, - &robot_state.dq, - &robot_state.I_load, - robot_state.m_total, - &robot_state.F_x_Ctotal, - ) - } + fn coriolis_from_state(&self, robot_state: &RobotState) -> [f64; 7]; + /// Calculates the gravity vector. Unit: \[Nm\]. /// # Arguments /// * `q` - Joint position. @@ -426,19 +254,15 @@ impl RobotModel for PandaModel { /// 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] { - let gravity_earth = gravity_earth.into().unwrap_or(&[0., 0., -9.81]); - let mut output = [0.; 7]; - self.library - .gravity(q, gravity_earth, &m_total, F_x_Ctotal, &mut output); - output - } + ) -> [f64; 7]; + /// Calculates the gravity vector. Unit: \[Nm\]. /// # Arguments /// * `robot_state` - State from which the gravity vector should be calculated. @@ -446,70 +270,27 @@ impl RobotModel for PandaModel { /// 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] { - self.gravity( - &robot_state.q, - robot_state.m_total, - &robot_state.F_x_Ctotal, - gravity_earth.into().unwrap_or(&robot_state.O_ddP_O), - ) - } + ) -> [f64; 7]; } -/// Calculates poses of joints and dynamic properties of the robot. +/// Calculates poses of joints and dynamic properties of a [Fr3](crate::Fr3). pub struct Fr3Model { library: ModelLibrary, } #[allow(non_snake_case)] impl RobotModel for Fr3Model { - /// 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`). - /// - /// If you do not have the model you can use the download_model example to download the model - /// # Arguments - /// * `model_filename` - Path to the model. - /// * `libm_filename` - Path to libm.so.6 . You probably do not need to specify the path as it - /// it should be detected automatically. However if you get panics for unresolved symbols or libm could - /// not be found you have to specify the path. On most Ubuntu systems it is in - /// ```text - /// /lib/x86_64-linux-gnu/libm.so.6 - /// ``` - /// It maybe has a different name on your machine but it is not the "libm.so". You can - /// verify that it is the correct libm by checking: - /// ```bash - /// nm -D /lib/x86_64-linux-gnu/libm.so.6 | grep sincos - /// ``` - /// # Errors - /// * ModelException - /// # libm FAQ - /// What is libm? - Libm is the Math library of the C Standard Library. It is what you get when - /// you include in C - /// - /// 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 \ ¯\_(ツ)_/¯ fn new>(model_filename: S, libm_filename: Option<&Path>) -> FrankaResult { Ok(Fr3Model { library: ModelLibrary::new(model_filename.as_ref(), libm_filename)?, }) } - /// 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. - /// * `q` - Joint position. - /// * `F_T_EE` - End effector in flange frame. - /// * `EE_T_K` - Stiffness frame K in the end effector frame. - /// # Return - /// Vectorized 4x4 pose matrix, column-major. + fn pose( &self, frame: &Frame, @@ -540,14 +321,7 @@ impl RobotModel for Fr3Model { } 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. + fn pose_from_state(&self, frame: &Frame, robot_state: &RobotState) -> [f64; 16] { self.pose( frame, @@ -556,16 +330,7 @@ impl RobotModel for Fr3Model { &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. + fn body_jacobian( &self, frame: &Frame, @@ -598,14 +363,6 @@ impl RobotModel for Fr3Model { 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. fn body_jacobian_from_state(&self, frame: &Frame, robot_state: &RobotState) -> [f64; 42] { self.body_jacobian( frame, @@ -614,16 +371,7 @@ impl RobotModel for Fr3Model { &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. + fn zero_jacobian( &self, frame: &Frame, @@ -655,14 +403,7 @@ impl RobotModel for Fr3Model { } 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. + fn zero_jacobian_from_state(&self, frame: &Frame, robot_state: &RobotState) -> [f64; 42] { self.zero_jacobian( frame, @@ -671,15 +412,7 @@ impl RobotModel for Fr3Model { &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. + fn mass( &self, q: &[f64; 7], @@ -692,11 +425,7 @@ impl RobotModel for Fr3Model { .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. + fn mass_from_state(&self, robot_state: &RobotState) -> [f64; 49] { self.mass( &robot_state.q, @@ -705,17 +434,7 @@ impl RobotModel for Fr3Model { &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. + fn coriolis( &self, q: &[f64; 7], @@ -730,12 +449,6 @@ impl RobotModel for Fr3Model { 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. fn coriolis_from_state(&self, robot_state: &RobotState) -> [f64; 7] { self.coriolis( &robot_state.q, @@ -745,15 +458,7 @@ impl RobotModel for Fr3Model { &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. + fn gravity<'a, Grav: Into>>( &self, q: &[f64; 7], @@ -767,13 +472,7 @@ impl RobotModel for Fr3Model { .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. + fn gravity_from_state<'a, Grav: Into>>( &self, robot_state: &RobotState, @@ -787,3 +486,6 @@ impl RobotModel for Fr3Model { ) } } + +/// Calculates poses of joints and dynamic properties of a [Panda](crate::Panda). +pub type PandaModel = Fr3Model; diff --git a/src/model/library_downloader.rs b/src/model/library_downloader.rs index 5218ef2..a770d41 100644 --- a/src/model/library_downloader.rs +++ b/src/model/library_downloader.rs @@ -1,5 +1,10 @@ // 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::robot_data::RobotData; @@ -8,20 +13,16 @@ use crate::robot::service_types::{ LoadModelLibrarySystem, }; use crate::FrankaResult; -use std::fmt; -use std::fmt::Display; -use std::fmt::Formatter; -use std::fs::File; -use std::io::Write; -use std::path::Path; -pub trait LibraryDownloader { +pub(crate) trait LibraryDownloader { type Data: RobotData; fn download(network: &mut Network, download_path: &Path) -> FrankaResult<()>; } -pub struct LibraryDownloaderGeneric { + +pub(crate) struct LibraryDownloaderGeneric { data: std::marker::PhantomData, } + impl LibraryDownloader for LibraryDownloaderGeneric { type Data = Data; @@ -53,18 +54,3 @@ impl LibraryDownloader for LibraryDownloaderGeneric { } } } - -#[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" - ) - } -} diff --git a/src/model/model_library.rs b/src/model/model_library.rs index ad7a489..1ec5d58 100644 --- a/src/model/model_library.rs +++ b/src/model/model_library.rs @@ -10,7 +10,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< From d9b74fca433df328bbbd3df61652abb43ca7ecc0 Mon Sep 17 00:00:00 2001 From: Marco Boneberger Date: Fri, 5 May 2023 15:20:54 +0200 Subject: [PATCH 33/63] wip introduce PrivateRobotData trait --- src/lib.rs | 2 +- src/model/library_downloader.rs | 8 +++---- src/robot/control_loop.rs | 26 ++++++++++---------- src/robot/fr3.rs | 20 ++++++++++++---- src/robot/panda.rs | 20 ++++++++++++---- src/robot/robot_data.rs | 16 +++++++++---- src/robot/robot_impl.rs | 42 ++++++++++++++++----------------- src/robot/robot_trait.rs | 11 +++++---- src/robot/robot_wrapper.rs | 36 ++++++++++++++-------------- 9 files changed, 105 insertions(+), 76 deletions(-) diff --git a/src/lib.rs b/src/lib.rs index 580c527..092417f 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -184,7 +184,7 @@ pub use robot::fr3::Fr3; pub use robot::low_pass_filter::DEFAULT_CUTOFF_FREQUENCY; pub use robot::low_pass_filter::MAX_CUTOFF_FREQUENCY; pub use robot::panda::Panda; -pub use robot::robot_data::RobotData; +// pub use robot::robot_data::PrivateRobotData; pub use robot::robot_state::RobotState; pub use robot::robot_wrapper::RobotWrapper; pub use utils::*; diff --git a/src/model/library_downloader.rs b/src/model/library_downloader.rs index a770d41..c0b6281 100644 --- a/src/model/library_downloader.rs +++ b/src/model/library_downloader.rs @@ -7,7 +7,7 @@ use std::path::Path; use crate::exception::FrankaException::ModelException; use crate::network::Network; -use crate::robot::robot_data::RobotData; +use crate::robot::robot_data::PrivateRobotData; use crate::robot::service_types::{ LoadModelLibraryArchitecture, LoadModelLibraryRequest, LoadModelLibraryStatus, LoadModelLibrarySystem, @@ -15,15 +15,15 @@ use crate::robot::service_types::{ use crate::FrankaResult; pub(crate) trait LibraryDownloader { - type Data: RobotData; + type Data: PrivateRobotData; fn download(network: &mut Network, download_path: &Path) -> FrankaResult<()>; } -pub(crate) struct LibraryDownloaderGeneric { +pub(crate) struct LibraryDownloaderGeneric { data: std::marker::PhantomData, } -impl LibraryDownloader for LibraryDownloaderGeneric { +impl LibraryDownloader for LibraryDownloaderGeneric { type Data = Data; fn download(network: &mut Network, download_path: &Path) -> FrankaResult<()> { diff --git a/src/robot/control_loop.rs b/src/robot/control_loop.rs index 44788f4..beb941c 100644 --- a/src/robot/control_loop.rs +++ b/src/robot/control_loop.rs @@ -11,7 +11,7 @@ use crate::robot::control_types::{ControllerMode, ConvertMotion, RealtimeConfig, 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_data::RobotData; +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}; @@ -22,18 +22,19 @@ type ControlCallback<'b, State> = &'b mut dyn FnMut(&State, &Duration) -> Torque pub struct ControlLoop< 'a, 'b, - T: RobotImplementation, - U: ConvertMotion<<::Data as RobotData>::State> + Data : RobotData, + T: RobotImplementation, + U: ConvertMotion<::State> + Debug + MotionGeneratorTrait + Finishable, - F: FnMut(&<::Data as RobotData>::State, &Duration) -> U, + F: FnMut(&::State, &Duration) -> U, > { pub default_deviation: MoveDeviation, robot: &'a mut T, motion_callback: F, control_callback: - Option::Data as RobotData>::State>>, + Option::State>>, limit_rate: bool, cutoff_frequency: f64, pub motion_id: u32, @@ -42,19 +43,20 @@ pub struct ControlLoop< impl< 'a, 'b, - T: RobotImplementation, - U: ConvertMotion<<::Data as RobotData>::State> + Data: RobotData, + T: RobotImplementation, + U: ConvertMotion<::State> + Debug + MotionGeneratorTrait + Finishable, - F: FnMut(&<::Data as RobotData>::State, &Duration) -> U, - > ControlLoop<'a, 'b, T, U, F> + F: FnMut(&::State, &Duration) -> U, + > ControlLoop<'a, 'b, Data, T, U, F> { pub fn new( robot: &'a mut T, control_callback: ControlCallback< 'b, - <::Data as RobotData>::State, + ::State, >, motion_callback: F, limit_rate: bool, @@ -100,7 +102,7 @@ impl< robot: &'a mut T, motion_callback: F, control_callback: Option< - ControlCallback<'b, <::Data as RobotData>::State>, + ControlCallback<'b, ::State>, >, limit_rate: bool, cutoff_frequency: f64, @@ -191,7 +193,7 @@ impl< fn spin_control( &mut self, - robot_state: &<::Data as RobotData>::State, + robot_state: &::State, time_step: &Duration, command: &mut ControllerCommand, ) -> bool { diff --git a/src/robot/fr3.rs b/src/robot/fr3.rs index 2edcc0d..f037af4 100644 --- a/src/robot/fr3.rs +++ b/src/robot/fr3.rs @@ -3,7 +3,7 @@ use crate::exception::{create_command_exception, FrankaException}; use crate::network::Network; use crate::robot::errors::FrankaErrors; use crate::robot::logger::Record; -use crate::robot::robot_data::RobotData; +use crate::robot::robot_data::{PrivateRobotData, RobotData}; use crate::robot::robot_impl::RobotImplGeneric; use crate::robot::robot_trait::{PrivateRobot, Robot}; use crate::robot::service_types; @@ -23,7 +23,11 @@ pub struct Fr3 { robimpl: RobotImplGeneric, } + impl PrivateRobot for Fr3 { + type Rob = RobotImplGeneric; + type PrivateData = Fr3Data; + fn get_rob_mut(&mut self) -> &mut Self::Rob { &mut self.robimpl } @@ -38,7 +42,7 @@ impl PrivateRobot for Fr3 { impl Robot for Fr3 { type Data = Fr3Data; - type Rob = RobotImplGeneric; + // type Rob = RobotImplGeneric; } impl Fr3 { @@ -55,7 +59,7 @@ impl Fr3 { } })?; Ok(Fr3 { - robimpl: ::Rob::new(network, log_size, realtime_config)?, + robimpl: RobotImplGeneric::new(network, log_size, realtime_config)?, }) } } @@ -81,11 +85,17 @@ impl DeviceData for Fr3Data { } impl RobotData for Fr3Data { - type DeviceData = Self; - type Header = Fr3CommandHeader; type State = RobotState; type StateIntern = Fr3StateIntern; type Model = Fr3Model; +} + +impl PrivateRobotData for Fr3Data { + // type DeviceData = Self; + type Header = Fr3CommandHeader; + // type State = RobotState; + // type StateIntern = Fr3StateIntern; + // type Model = Fr3Model; type LoadModelRequestWithHeader = LoadModelLibraryRequestWithFr3Header; type SetCollisionBehaviorRequestWithHeader = SetCollisionBehaviorRequestWithFr3Header; type SetLoadRequestWithHeader = SetLoadRequestWithFr3Header; diff --git a/src/robot/panda.rs b/src/robot/panda.rs index a7e69bb..d5160cc 100644 --- a/src/robot/panda.rs +++ b/src/robot/panda.rs @@ -3,7 +3,7 @@ use crate::exception::{create_command_exception, FrankaException}; use crate::network::Network; use crate::robot::errors::FrankaErrors; use crate::robot::logger::Record; -use crate::robot::robot_data::RobotData; +use crate::robot::robot_data::{PrivateRobotData, RobotData}; use crate::robot::robot_impl::RobotImplGeneric; use crate::robot::robot_trait::{PrivateRobot, Robot}; use crate::robot::service_types; @@ -116,7 +116,7 @@ impl Panda { } })?; Ok(Panda { - robimpl: ::Rob::new(network, log_size, realtime_config)?, + robimpl: RobotImplGeneric::new(network, log_size, realtime_config)?, }) } @@ -202,10 +202,14 @@ impl Panda { impl Robot for Panda { type Data = PandaData; - type Rob = RobotImplGeneric; + // type Rob = RobotImplGeneric; } impl PrivateRobot for Panda { + // type Rob = ; + type Rob = RobotImplGeneric; + type PrivateData = PandaData; + fn get_rob_mut(&mut self) -> &mut Self::Rob { &mut self.robimpl } @@ -239,11 +243,17 @@ impl DeviceData for PandaData { } impl RobotData for PandaData { - type DeviceData = Self; - type Header = PandaCommandHeader; type State = RobotState; type StateIntern = PandaStateIntern; type Model = PandaModel; +} + +impl PrivateRobotData for PandaData { + // type DeviceData = Self; + type Header = PandaCommandHeader; + // type State = RobotState; + // type StateIntern = PandaStateIntern; + // type Model = PandaModel; type LoadModelRequestWithHeader = LoadModelLibraryRequestWithPandaHeader; type SetCollisionBehaviorRequestWithHeader = SetCollisionBehaviorRequestWithPandaHeader; type SetLoadRequestWithHeader = SetLoadRequestWithPandaHeader; diff --git a/src/robot/robot_data.rs b/src/robot/robot_data.rs index 81fab95..c710325 100644 --- a/src/robot/robot_data.rs +++ b/src/robot/robot_data.rs @@ -15,12 +15,18 @@ use serde::de::DeserializeOwned; use serde::Serialize; use std::fmt::Debug; -pub trait RobotData: DeviceData { - type DeviceData: DeviceData; +pub trait RobotData { + type Model: RobotModel; + type StateIntern: Debug + DeserializeOwned + Serialize + AbstractRobotStateIntern + 'static; + type State: AbstractRobotState + From + From; +} + +pub(crate) trait PrivateRobotData: DeviceData + RobotData { + // type DeviceData: DeviceData; type Header: RobotHeader; - type State: AbstractRobotState + From + From; - type StateIntern: Debug + DeserializeOwned + Serialize + AbstractRobotStateIntern + 'static; - type Model: RobotModel; + // type State: AbstractRobotState + From + From; + // type StateIntern: Debug + DeserializeOwned + Serialize + AbstractRobotStateIntern + 'static; + // type Model: RobotModel; type LoadModelRequestWithHeader: MessageCommand + Serialize + From<(u32, LoadModelLibraryRequest)>; diff --git a/src/robot/robot_impl.rs b/src/robot/robot_impl.rs index 76cbaf3..372dea7 100644 --- a/src/robot/robot_impl.rs +++ b/src/robot/robot_impl.rs @@ -7,7 +7,7 @@ use crate::robot::control_types::RealtimeConfig; use crate::robot::logger::Logger; use crate::robot::robot_control::RobotControl; -use crate::robot::robot_data::RobotData; +use crate::robot::robot_data::{PrivateRobotData, RobotData}; use crate::robot::robot_state::AbstractRobotState; use crate::robot::service_types::{ ConnectResponseWithoutHeader, ConnectStatus, MoveControllerMode, MoveDeviation, @@ -20,24 +20,24 @@ use crate::robot::types::{ use crate::RobotModel; use std::fs::remove_file; use std::path::Path; +// use crate::robot::robot_trait::PrivateRobot; -pub trait RobotImplementation: - RobotControl::Data as RobotData>::State> +pub trait RobotImplementation: + RobotControl::State> { - type Data: RobotData; fn read_once( &mut self, - ) -> FrankaResult<<::Data as RobotData>::State>; + ) -> FrankaResult<::State>; fn load_model( &mut self, persistent: bool, - ) -> FrankaResult<<::Data as RobotData>::Model>; + ) -> FrankaResult<::Model>; fn server_version(&self) -> u16; } -pub struct RobotImplGeneric { - pub network: Network, - logger: Logger, +pub(crate) struct RobotImplGeneric { + pub network: Network, + logger: Logger<::State>, realtime_config: RealtimeConfig, ri_version: Option, motion_generator_mode: Option, @@ -47,7 +47,7 @@ pub struct RobotImplGeneric { message_id: u64, } -impl RobotControl for RobotImplGeneric { +impl RobotControl for RobotImplGeneric { type State = Data::State; fn start_motion( @@ -222,14 +222,14 @@ impl RobotControl for RobotImplGeneric { } } -impl> RobotImplementation +impl RobotImplementation for RobotImplGeneric { - type Data = Data; + // type Data = Data; fn read_once( &mut self, - ) -> FrankaResult<<::Data as RobotData>::State> { + ) -> FrankaResult { while self.network.udp_receive::().is_some() {} Ok(Data::State::from(self.receive_robot_state()?)) } @@ -251,9 +251,9 @@ impl> RobotImplementation } } -impl RobotImplGeneric { +impl RobotImplGeneric { pub fn new( - network: Network, + network: Network, log_size: usize, realtime_config: RealtimeConfig, ) -> FrankaResult { @@ -302,10 +302,10 @@ impl RobotImplGeneric { self.controller_mode = robot_state.get_controller_mode(); self.message_id = robot_state.get_message_id(); } - pub fn read_once(&mut self) -> FrankaResult { - while self.network.udp_receive::().is_some() {} - Ok(Data::State::from(self.receive_robot_state()?)) - } + // pub fn read_once(&mut self) -> FrankaResult { + // while self.network.udp_receive::().is_some() {} + // Ok(Data::State::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::(); @@ -388,9 +388,7 @@ impl RobotImplGeneric { fn controller_running(&self) -> bool { self.controller_mode == ControllerMode::ExternalController } - pub fn server_version(&self) -> u16 { - self.ri_version.unwrap() - } + fn execute_move_command( &mut self, controller_mode: &MoveControllerMode, diff --git a/src/robot/robot_trait.rs b/src/robot/robot_trait.rs index 4697a53..cf93b63 100644 --- a/src/robot/robot_trait.rs +++ b/src/robot/robot_trait.rs @@ -1,7 +1,7 @@ use crate::network::Network; use crate::robot::control_loop::ControlLoop; use crate::robot::motion_generator_traits::MotionGeneratorTrait; -use crate::robot::robot_data::RobotData; +use crate::robot::robot_data::{PrivateRobotData, RobotData}; use crate::robot::robot_impl::RobotImplementation; use crate::{ CartesianPose, CartesianVelocities, ControllerMode, ConvertMotion, Finishable, FrankaResult, @@ -17,10 +17,13 @@ where JointPositions: ConvertMotion<<::Data as RobotData>::State>, CartesianVelocities: ConvertMotion<<::Data as RobotData>::State>, RobotState: From<<::Data as RobotData>::State>, + { + type Rob: RobotImplementation; + type PrivateData: PrivateRobotData; fn get_rob_mut(&mut self) -> &mut Self::Rob; - fn get_rob(&self) -> &Self::Rob; - fn get_net(&mut self) -> &mut Network; + fn get_rob(&self) -> & Self::Rob; + fn get_net(&mut self) -> &mut Network; fn control_motion_intern< F: FnMut(&<::Data as RobotData>::State, &Duration) -> U, @@ -86,5 +89,5 @@ where RobotState: From<<::Data as RobotData>::State>, { type Data: RobotData; - type Rob: RobotImplementation; + } diff --git a/src/robot/robot_wrapper.rs b/src/robot/robot_wrapper.rs index 62cfd26..59438b9 100644 --- a/src/robot/robot_wrapper.rs +++ b/src/robot/robot_wrapper.rs @@ -1,6 +1,6 @@ use crate::exception::create_command_exception; use crate::robot::robot_control::RobotControl; -use crate::robot::robot_data::RobotData; +use crate::robot::robot_data::{PrivateRobotData, RobotData}; use crate::robot::robot_impl::RobotImplementation; use crate::robot::robot_trait::PrivateRobot; use crate::robot::robot_trait::Robot; @@ -638,7 +638,7 @@ where lower_force_thresholds_nominal: [f64; 6], upper_force_thresholds_nominal: [f64; 6], ) -> FrankaResult<()> { - let command = ::Data::create_set_collision_behavior_request( + let command = ::PrivateData::create_set_collision_behavior_request( &mut ::get_net(self).command_id, SetCollisionBehaviorRequest::new( lower_torque_thresholds_acceleration, @@ -653,7 +653,7 @@ where ); let command_id: u32 = ::get_net(self).tcp_send_request(command); let status = ::get_net(self).tcp_blocking_receive_status(command_id); - ::Data::handle_getter_setter_status(status) + ::PrivateData::handle_getter_setter_status(status) } fn set_default_behavior(&mut self) -> FrankaResult<()> { @@ -667,24 +667,24 @@ where #[allow(non_snake_case)] fn set_joint_impedance(&mut self, K_theta: [f64; 7]) -> FrankaResult<()> { - let command = ::Data::create_set_joint_impedance_request( + let command = ::PrivateData::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); - ::Data::handle_getter_setter_status(status) + ::PrivateData::handle_getter_setter_status(status) } #[allow(non_snake_case)] fn set_cartesian_impedance(&mut self, K_x: [f64; 6]) -> FrankaResult<()> { - let command = ::Data::create_set_cartesian_impedance_request( + let command = ::PrivateData::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); - ::Data::handle_getter_setter_status(status) + ::PrivateData::handle_getter_setter_status(status) } #[allow(non_snake_case)] @@ -694,58 +694,58 @@ where F_x_Cload: [f64; 3], load_inertia: [f64; 9], ) -> FrankaResult<()> { - let command = ::Data::create_set_load_request( + let command = ::PrivateData::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); - ::Data::handle_getter_setter_status(status) + ::PrivateData::handle_getter_setter_status(status) } fn set_guiding_mode(&mut self, guiding_mode: [bool; 6], elbow: bool) -> FrankaResult<()> { - let command = ::Data::create_set_guiding_mode_request( + let command = ::PrivateData::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); - ::Data::handle_getter_setter_status(status) + ::PrivateData::handle_getter_setter_status(status) } #[allow(non_snake_case)] fn set_K(&mut self, EE_T_K: [f64; 16]) -> FrankaResult<()> { - let command = ::Data::create_set_ee_to_k_request( + let command = ::PrivateData::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); - ::Data::handle_getter_setter_status(status) + ::PrivateData::handle_getter_setter_status(status) } #[allow(non_snake_case)] fn set_EE(&mut self, NE_T_EE: [f64; 16]) -> FrankaResult<()> { - let command = ::Data::create_set_ne_to_ee_request( + let command = ::PrivateData::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); - ::Data::handle_getter_setter_status(status) + ::PrivateData::handle_getter_setter_status(status) } fn automatic_error_recovery(&mut self) -> FrankaResult<()> { - let command = ::Data::create_automatic_error_recovery_request( + let command = ::PrivateData::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); - ::Data::handle_automatic_error_recovery_status(status) + ::PrivateData::handle_automatic_error_recovery_status(status) } fn stop(&mut self) -> FrankaResult<()> { - let command = ::Data::create_stop_request( + let command = ::PrivateData::create_stop_request( &mut ::get_net(self).command_id, ); let command_id: u32 = ::get_net(self).tcp_send_request(command); From aca989cbbd02a8af3529661aafc98926f77f76f9 Mon Sep 17 00:00:00 2001 From: Marco Boneberger Date: Fri, 5 May 2023 17:33:32 +0200 Subject: [PATCH 34/63] remove Robot trait --- src/device_data.rs | 2 +- src/lib.rs | 1 - src/network.rs | 2 +- src/robot/control_loop.rs | 28 ++++--------- src/robot/fr3.rs | 55 +++++++++---------------- src/robot/panda.rs | 71 ++++++++++++-------------------- src/robot/robot_data.rs | 10 ++--- src/robot/robot_impl.rs | 22 +++------- src/robot/robot_trait.rs | 51 +++++------------------ src/robot/robot_wrapper.rs | 83 +++++++++++++++++--------------------- 10 files changed, 112 insertions(+), 213 deletions(-) diff --git a/src/device_data.rs b/src/device_data.rs index e10becb..d93f827 100644 --- a/src/device_data.rs +++ b/src/device_data.rs @@ -1,6 +1,6 @@ use crate::gripper::types::CommandHeader; -pub trait DeviceData { +pub(crate) trait DeviceData { type CommandHeader: CommandHeader; type CommandEnum; fn create_header( diff --git a/src/lib.rs b/src/lib.rs index 092417f..57ecbb9 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -184,7 +184,6 @@ pub use robot::fr3::Fr3; pub use robot::low_pass_filter::DEFAULT_CUTOFF_FREQUENCY; pub use robot::low_pass_filter::MAX_CUTOFF_FREQUENCY; pub use robot::panda::Panda; -// pub use robot::robot_data::PrivateRobotData; pub use robot::robot_state::RobotState; pub use robot::robot_wrapper::RobotWrapper; pub use utils::*; diff --git a/src/network.rs b/src/network.rs index f93c423..0fd27fb 100644 --- a/src/network.rs +++ b/src/network.rs @@ -45,7 +45,7 @@ pub trait MessageCommand { fn get_command_message_id(&self) -> u32; } -pub struct Network { +pub(crate) struct Network { tcp_socket: TcpStream, udp_socket: UdpSocket, udp_server_address: SocketAddr, diff --git a/src/robot/control_loop.rs b/src/robot/control_loop.rs index beb941c..415173d 100644 --- a/src/robot/control_loop.rs +++ b/src/robot/control_loop.rs @@ -11,7 +11,7 @@ use crate::robot::control_types::{ControllerMode, ConvertMotion, RealtimeConfig, 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_data::{ RobotData}; +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}; @@ -22,19 +22,15 @@ type ControlCallback<'b, State> = &'b mut dyn FnMut(&State, &Duration) -> Torque pub struct ControlLoop< 'a, 'b, - Data : RobotData, + Data: RobotData, T: RobotImplementation, - U: ConvertMotion<::State> - + Debug - + MotionGeneratorTrait - + Finishable, + U: ConvertMotion<::State> + Debug + MotionGeneratorTrait + Finishable, F: FnMut(&::State, &Duration) -> U, > { pub default_deviation: MoveDeviation, robot: &'a mut T, motion_callback: F, - control_callback: - Option::State>>, + control_callback: Option::State>>, limit_rate: bool, cutoff_frequency: f64, pub motion_id: u32, @@ -43,21 +39,15 @@ pub struct ControlLoop< impl< 'a, 'b, - Data: RobotData, + Data: RobotData, T: RobotImplementation, - U: ConvertMotion<::State> - + Debug - + MotionGeneratorTrait - + Finishable, + U: ConvertMotion<::State> + Debug + MotionGeneratorTrait + Finishable, F: FnMut(&::State, &Duration) -> U, > ControlLoop<'a, 'b, Data, T, U, F> { pub fn new( robot: &'a mut T, - control_callback: ControlCallback< - 'b, - ::State, - >, + control_callback: ControlCallback<'b, ::State>, motion_callback: F, limit_rate: bool, cutoff_frequency: f64, @@ -101,9 +91,7 @@ impl< fn new_intern( robot: &'a mut T, motion_callback: F, - control_callback: Option< - ControlCallback<'b, ::State>, - >, + control_callback: Option::State>>, limit_rate: bool, cutoff_frequency: f64, ) -> FrankaResult { diff --git a/src/robot/fr3.rs b/src/robot/fr3.rs index f037af4..6f85262 100644 --- a/src/robot/fr3.rs +++ b/src/robot/fr3.rs @@ -5,7 +5,7 @@ use crate::robot::errors::FrankaErrors; use crate::robot::logger::Record; use crate::robot::robot_data::{PrivateRobotData, RobotData}; use crate::robot::robot_impl::RobotImplGeneric; -use crate::robot::robot_trait::{PrivateRobot, Robot}; +use crate::robot::robot_trait::PrivateRobot; use crate::robot::service_types; use crate::robot::service_types::{ AutomaticErrorRecoveryStatusFr3, ConnectRequest, ConnectRequestWithFr3Header, Fr3CommandEnum, @@ -19,32 +19,29 @@ use crate::robot::types::Fr3StateIntern; use crate::{Fr3Model, FrankaResult, RealtimeConfig, RobotState}; use std::mem::size_of; -pub struct Fr3 { - robimpl: RobotImplGeneric, -} +pub struct Fr3(RobotImplGeneric); +impl RobotData for Fr3 { + type Model = Fr3Model; + type StateIntern = Fr3StateIntern; + type State = RobotState; +} impl PrivateRobot for Fr3 { - type Rob = RobotImplGeneric; - type PrivateData = Fr3Data; + type Rob = RobotImplGeneric; fn get_rob_mut(&mut self) -> &mut Self::Rob { - &mut self.robimpl + &mut self.0 } fn get_rob(&self) -> &Self::Rob { - &self.robimpl + &self.0 } - fn get_net(&mut self) -> &mut Network { - &mut self.robimpl.network + fn get_net(&mut self) -> &mut Network { + &mut self.0.network } } -impl Robot for Fr3 { - type Data = Fr3Data; - // type Rob = RobotImplGeneric; -} - impl Fr3 { pub fn new>, LogSize: Into>>( franka_address: &str, @@ -58,15 +55,15 @@ impl Fr3 { message: "Connection could not be established".to_string(), } })?; - Ok(Fr3 { - robimpl: RobotImplGeneric::new(network, log_size, realtime_config)?, - }) + Ok(Fr3(RobotImplGeneric::new( + network, + log_size, + realtime_config, + )?)) } } -pub struct Fr3Data {} - -impl DeviceData for Fr3Data { +impl DeviceData for Fr3 { type CommandHeader = Fr3CommandHeader; type CommandEnum = Fr3CommandEnum; fn create_header( @@ -84,18 +81,8 @@ impl DeviceData for Fr3Data { } } -impl RobotData for Fr3Data { - type State = RobotState; - type StateIntern = Fr3StateIntern; - type Model = Fr3Model; -} - -impl PrivateRobotData for Fr3Data { - // type DeviceData = Self; +impl PrivateRobotData for Fr3 { type Header = Fr3CommandHeader; - // type State = RobotState; - // type StateIntern = Fr3StateIntern; - // type Model = Fr3Model; type LoadModelRequestWithHeader = LoadModelLibraryRequestWithFr3Header; type SetCollisionBehaviorRequestWithHeader = SetCollisionBehaviorRequestWithFr3Header; type SetLoadRequestWithHeader = SetLoadRequestWithFr3Header; @@ -107,12 +94,10 @@ impl PrivateRobotData for Fr3Data { 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, diff --git a/src/robot/panda.rs b/src/robot/panda.rs index d5160cc..5beda14 100644 --- a/src/robot/panda.rs +++ b/src/robot/panda.rs @@ -5,7 +5,7 @@ use crate::robot::errors::FrankaErrors; use crate::robot::logger::Record; use crate::robot::robot_data::{PrivateRobotData, RobotData}; use crate::robot::robot_impl::RobotImplGeneric; -use crate::robot::robot_trait::{PrivateRobot, Robot}; +use crate::robot::robot_trait::PrivateRobot; use crate::robot::service_types; use crate::robot::service_types::{ AutomaticErrorRecoveryStatusPanda, ConnectRequest, ConnectRequestWithPandaHeader, @@ -76,9 +76,7 @@ use std::mem::size_of; /// /// 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 Panda { - robimpl: RobotImplGeneric, -} +pub struct Panda(RobotImplGeneric); impl Panda { /// Establishes a connection with the robot. @@ -115,9 +113,11 @@ impl Panda { message: "Connection could not be established".to_string(), } })?; - Ok(Panda { - robimpl: RobotImplGeneric::new(network, log_size, realtime_config)?, - }) + Ok(Panda(RobotImplGeneric::new( + network, + log_size, + realtime_config, + )?)) } /// Sets the cut off frequency for the given motion generator or controller. @@ -147,7 +147,7 @@ impl Panda { controller_filter_frequency: f64, ) -> FrankaResult<()> { let command = SetFiltersRequestWithPandaHeader { - header: self.robimpl.network.create_header_for_panda( + header: self.0.network.create_header_for_panda( PandaCommandEnum::SetFilters, size_of::(), ), @@ -159,12 +159,9 @@ impl Panda { 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); - PandaData::handle_getter_setter_status(response.status) + 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. @@ -177,17 +174,15 @@ impl Panda { /// * [`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.robimpl.network.create_header_for_panda( + header: self.0.network.create_header_for_panda( PandaCommandEnum::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); + 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( @@ -200,31 +195,26 @@ impl Panda { } } -impl Robot for Panda { - type Data = PandaData; - // type Rob = RobotImplGeneric; -} - impl PrivateRobot for Panda { // type Rob = ; - type Rob = RobotImplGeneric; - type PrivateData = PandaData; + type Rob = RobotImplGeneric; + // type PrivateData = Panda; fn get_rob_mut(&mut self) -> &mut Self::Rob { - &mut self.robimpl + &mut self.0 } fn get_rob(&self) -> &Self::Rob { - &self.robimpl + &self.0 } - fn get_net(&mut self) -> &mut Network { - &mut self.robimpl.network + fn get_net(&mut self) -> &mut Network { + &mut self.0.network } } pub struct PandaData {} -impl DeviceData for PandaData { +impl DeviceData for Panda { type CommandHeader = PandaCommandHeader; type CommandEnum = PandaCommandEnum; fn create_header( @@ -242,18 +232,14 @@ impl DeviceData for PandaData { } } -impl RobotData for PandaData { - type State = RobotState; - type StateIntern = PandaStateIntern; +impl RobotData for Panda { type Model = PandaModel; + type StateIntern = PandaStateIntern; + type State = RobotState; } -impl PrivateRobotData for PandaData { - // type DeviceData = Self; +impl PrivateRobotData for Panda { type Header = PandaCommandHeader; - // type State = RobotState; - // type StateIntern = PandaStateIntern; - // type Model = PandaModel; type LoadModelRequestWithHeader = LoadModelLibraryRequestWithPandaHeader; type SetCollisionBehaviorRequestWithHeader = SetCollisionBehaviorRequestWithPandaHeader; type SetLoadRequestWithHeader = SetLoadRequestWithPandaHeader; @@ -263,15 +249,10 @@ impl PrivateRobotData for PandaData { 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( diff --git a/src/robot/robot_data.rs b/src/robot/robot_data.rs index c710325..bea80fa 100644 --- a/src/robot/robot_data.rs +++ b/src/robot/robot_data.rs @@ -16,17 +16,13 @@ use serde::Serialize; use std::fmt::Debug; pub trait RobotData { - type Model: RobotModel; - type StateIntern: Debug + DeserializeOwned + Serialize + AbstractRobotStateIntern + 'static; - type State: AbstractRobotState + From + From; + type Model: RobotModel; + type StateIntern: Debug + DeserializeOwned + Serialize + AbstractRobotStateIntern + 'static; + type State: AbstractRobotState + From + From; } pub(crate) trait PrivateRobotData: DeviceData + RobotData { - // type DeviceData: DeviceData; type Header: RobotHeader; - // type State: AbstractRobotState + From + From; - // type StateIntern: Debug + DeserializeOwned + Serialize + AbstractRobotStateIntern + 'static; - // type Model: RobotModel; type LoadModelRequestWithHeader: MessageCommand + Serialize + From<(u32, LoadModelLibraryRequest)>; diff --git a/src/robot/robot_impl.rs b/src/robot/robot_impl.rs index 372dea7..3fdd0fd 100644 --- a/src/robot/robot_impl.rs +++ b/src/robot/robot_impl.rs @@ -20,18 +20,12 @@ use crate::robot::types::{ use crate::RobotModel; use std::fs::remove_file; use std::path::Path; -// use crate::robot::robot_trait::PrivateRobot; -pub trait RobotImplementation: +pub trait RobotImplementation: RobotControl::State> { - fn read_once( - &mut self, - ) -> FrankaResult<::State>; - fn load_model( - &mut self, - persistent: bool, - ) -> FrankaResult<::Model>; + fn read_once(&mut self) -> FrankaResult<::State>; + fn load_model(&mut self, persistent: bool) -> FrankaResult<::Model>; fn server_version(&self) -> u16; } @@ -48,7 +42,7 @@ pub(crate) struct RobotImplGeneric { } impl RobotControl for RobotImplGeneric { - type State = Data::State; + type State = ::State; fn start_motion( &mut self, @@ -222,14 +216,10 @@ impl RobotControl for RobotImplGeneric { } } -impl RobotImplementation - for RobotImplGeneric -{ +impl RobotImplementation for RobotImplGeneric { // type Data = Data; - fn read_once( - &mut self, - ) -> FrankaResult { + fn read_once(&mut self) -> FrankaResult { while self.network.udp_receive::().is_some() {} Ok(Data::State::from(self.receive_robot_state()?)) } diff --git a/src/robot/robot_trait.rs b/src/robot/robot_trait.rs index cf93b63..41d5533 100644 --- a/src/robot/robot_trait.rs +++ b/src/robot/robot_trait.rs @@ -4,33 +4,20 @@ use crate::robot::motion_generator_traits::MotionGeneratorTrait; use crate::robot::robot_data::{PrivateRobotData, RobotData}; use crate::robot::robot_impl::RobotImplementation; use crate::{ - CartesianPose, CartesianVelocities, ControllerMode, ConvertMotion, Finishable, FrankaResult, - JointPositions, JointVelocities, RobotState, Torques, DEFAULT_CUTOFF_FREQUENCY, + ControllerMode, ConvertMotion, Finishable, FrankaResult, Torques, DEFAULT_CUTOFF_FREQUENCY, }; use std::fmt::Debug; use std::time::Duration; -pub(crate) trait PrivateRobot: Robot -where - CartesianPose: ConvertMotion<<::Data as RobotData>::State>, - JointVelocities: ConvertMotion<<::Data as RobotData>::State>, - JointPositions: ConvertMotion<<::Data as RobotData>::State>, - CartesianVelocities: ConvertMotion<<::Data as RobotData>::State>, - RobotState: From<<::Data as RobotData>::State>, - -{ - type Rob: RobotImplementation; - type PrivateData: PrivateRobotData; +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 get_rob(&self) -> &Self::Rob; + fn get_net(&mut self) -> &mut Network; fn control_motion_intern< - F: FnMut(&<::Data as RobotData>::State, &Duration) -> U, - U: ConvertMotion<<::Data as RobotData>::State> - + Debug - + MotionGeneratorTrait - + Finishable, + F: FnMut(&::State, &Duration) -> U, + U: ConvertMotion<::State> + Debug + MotionGeneratorTrait + Finishable, >( &mut self, motion_generator_callback: F, @@ -52,18 +39,12 @@ where } fn control_torques_intern< - F: FnMut(&<::Data as RobotData>::State, &Duration) -> U, - U: ConvertMotion<<::Data as RobotData>::State> - + Debug - + MotionGeneratorTrait - + Finishable, + F: FnMut(&::State, &Duration) -> U, + U: ConvertMotion<::State> + Debug + MotionGeneratorTrait + Finishable, >( &mut self, motion_generator_callback: F, - control_callback: &mut dyn FnMut( - &<::Data as RobotData>::State, - &Duration, - ) -> Torques, + control_callback: &mut dyn FnMut(&::State, &Duration) -> Torques, limit_rate: Option, cutoff_frequency: Option, ) -> FrankaResult<()> { @@ -79,15 +60,3 @@ where control_loop.run() } } - -pub trait Robot -where - CartesianPose: ConvertMotion<<::Data as RobotData>::State>, - JointVelocities: ConvertMotion<<::Data as RobotData>::State>, - JointPositions: ConvertMotion<<::Data as RobotData>::State>, - CartesianVelocities: ConvertMotion<<::Data as RobotData>::State>, - RobotState: From<<::Data as RobotData>::State>, -{ - type Data: RobotData; - -} diff --git a/src/robot/robot_wrapper.rs b/src/robot/robot_wrapper.rs index 59438b9..0ea957f 100644 --- a/src/robot/robot_wrapper.rs +++ b/src/robot/robot_wrapper.rs @@ -3,7 +3,6 @@ use crate::robot::robot_control::RobotControl; use crate::robot::robot_data::{PrivateRobotData, RobotData}; use crate::robot::robot_impl::RobotImplementation; use crate::robot::robot_trait::PrivateRobot; -use crate::robot::robot_trait::Robot; use crate::robot::service_types::{ SetCartesianImpedanceRequest, SetCollisionBehaviorRequest, SetEeToKRequest, SetGuidingModeRequest, SetJointImpedanceRequest, SetLoadRequest, SetNeToEeRequest, @@ -567,13 +566,13 @@ pub trait RobotWrapper { impl RobotWrapper for R where - RobotState: From<<::Data as RobotData>::State>, - CartesianPose: crate::ConvertMotion<<::Data as RobotData>::State>, - JointVelocities: crate::ConvertMotion<<::Data as RobotData>::State>, - JointPositions: crate::ConvertMotion<<::Data as RobotData>::State>, - CartesianVelocities: crate::ConvertMotion<<::Data as RobotData>::State>, + RobotState: From<::State>, + CartesianPose: crate::ConvertMotion<::State>, + JointVelocities: crate::ConvertMotion<::State>, + JointPositions: crate::ConvertMotion<::State>, + CartesianVelocities: crate::ConvertMotion<::State>, { - type Model = <::Data as RobotData>::Model; + type Model = ::Model; fn read bool>(&mut self, mut read_callback: F) -> FrankaResult<()> { loop { @@ -597,7 +596,7 @@ where limit_rate: L, cutoff_frequency: CF, ) -> FrankaResult<()> { - let cb = |state: &<::Data as RobotData>::State, duration: &Duration| { + let cb = |state: &::State, duration: &Duration| { motion_generator_callback(&(state.clone().into()), duration) }; ::control_motion_intern( @@ -638,7 +637,7 @@ where lower_force_thresholds_nominal: [f64; 6], upper_force_thresholds_nominal: [f64; 6], ) -> FrankaResult<()> { - let command = ::PrivateData::create_set_collision_behavior_request( + let command = ::create_set_collision_behavior_request( &mut ::get_net(self).command_id, SetCollisionBehaviorRequest::new( lower_torque_thresholds_acceleration, @@ -653,7 +652,7 @@ where ); let command_id: u32 = ::get_net(self).tcp_send_request(command); let status = ::get_net(self).tcp_blocking_receive_status(command_id); - ::PrivateData::handle_getter_setter_status(status) + ::handle_getter_setter_status(status) } fn set_default_behavior(&mut self) -> FrankaResult<()> { @@ -667,24 +666,24 @@ where #[allow(non_snake_case)] fn set_joint_impedance(&mut self, K_theta: [f64; 7]) -> FrankaResult<()> { - let command = ::PrivateData::create_set_joint_impedance_request( + 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); - ::PrivateData::handle_getter_setter_status(status) + ::handle_getter_setter_status(status) } #[allow(non_snake_case)] fn set_cartesian_impedance(&mut self, K_x: [f64; 6]) -> FrankaResult<()> { - let command = ::PrivateData::create_set_cartesian_impedance_request( + 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); - ::PrivateData::handle_getter_setter_status(status) + ::handle_getter_setter_status(status) } #[allow(non_snake_case)] @@ -694,58 +693,58 @@ where F_x_Cload: [f64; 3], load_inertia: [f64; 9], ) -> FrankaResult<()> { - let command = ::PrivateData::create_set_load_request( + 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); - ::PrivateData::handle_getter_setter_status(status) + ::handle_getter_setter_status(status) } fn set_guiding_mode(&mut self, guiding_mode: [bool; 6], elbow: bool) -> FrankaResult<()> { - let command = ::PrivateData::create_set_guiding_mode_request( + 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); - ::PrivateData::handle_getter_setter_status(status) + ::handle_getter_setter_status(status) } #[allow(non_snake_case)] fn set_K(&mut self, EE_T_K: [f64; 16]) -> FrankaResult<()> { - let command = ::PrivateData::create_set_ee_to_k_request( + 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); - ::PrivateData::handle_getter_setter_status(status) + ::handle_getter_setter_status(status) } #[allow(non_snake_case)] fn set_EE(&mut self, NE_T_EE: [f64; 16]) -> FrankaResult<()> { - let command = ::PrivateData::create_set_ne_to_ee_request( + 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); - ::PrivateData::handle_getter_setter_status(status) + ::handle_getter_setter_status(status) } fn automatic_error_recovery(&mut self) -> FrankaResult<()> { - let command = ::PrivateData::create_automatic_error_recovery_request( + 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); - ::PrivateData::handle_automatic_error_recovery_status(status) + ::handle_automatic_error_recovery_status(status) } fn stop(&mut self) -> FrankaResult<()> { - let command = ::PrivateData::create_stop_request( + let command = ::create_stop_request( &mut ::get_net(self).command_id, ); let command_id: u32 = ::get_net(self).tcp_send_request(command); @@ -779,7 +778,7 @@ where limit_rate: L, cutoff_frequency: CF, ) -> FrankaResult<()> { - let cb = |state: &<::Data as RobotData>::State, duration: &Duration| { + let cb = |state: &::State, duration: &Duration| { motion_generator_callback(&(state.clone().into()), duration) // todo make this without cloning }; ::control_motion_intern( @@ -803,7 +802,7 @@ where limit_rate: L, cutoff_frequency: CF, ) -> FrankaResult<()> { - let cb = |state: &<::Data as RobotData>::State, duration: &Duration| { + let cb = |state: &::State, duration: &Duration| { motion_generator_callback(&(state.clone().into()), duration) }; ::control_motion_intern( @@ -827,7 +826,7 @@ where limit_rate: L, cutoff_frequency: CF, ) -> FrankaResult<()> { - let cb = |state: &<::Data as RobotData>::State, duration: &Duration| { + let cb = |state: &::State, duration: &Duration| { motion_generator_callback(&(state.clone().into()), duration) }; self.control_motion_intern( @@ -849,10 +848,10 @@ where cutoff_frequency: CF, ) -> FrankaResult<()> { let motion_generator_callback = - |_state: &<::Data as RobotData>::State, _time_step: &Duration| { + |_state: &::State, _time_step: &Duration| { JointVelocities::new([0.; 7]) }; - let mut cb = |state: &<::Data as RobotData>::State, duration: &Duration| { + let mut cb = |state: &::State, duration: &Duration| { control_callback(&(state.clone().into()), duration) }; self.control_torques_intern( @@ -875,12 +874,10 @@ where limit_rate: L, cutoff_frequency: CF, ) -> FrankaResult<()> { - let motion_generator_cb = |state: &<::Data as RobotData>::State, - duration: &Duration| { + let motion_generator_cb = |state: &::State, duration: &Duration| { motion_generator_callback(&(state.clone().into()), duration) }; - let mut control_cb = |state: &<::Data as RobotData>::State, - duration: &Duration| { + let mut control_cb = |state: &::State, duration: &Duration| { control_callback(&(state.clone().into()), duration) }; self.control_torques_intern( @@ -903,12 +900,10 @@ where limit_rate: L, cutoff_frequency: CF, ) -> FrankaResult<()> { - let motion_generator_cb = |state: &<::Data as RobotData>::State, - duration: &Duration| { + let motion_generator_cb = |state: &::State, duration: &Duration| { motion_generator_callback(&(state.clone().into()), duration) }; - let mut control_cb = |state: &<::Data as RobotData>::State, - duration: &Duration| { + let mut control_cb = |state: &::State, duration: &Duration| { control_callback(&(state.clone().into()), duration) }; self.control_torques_intern( @@ -931,12 +926,10 @@ where limit_rate: L, cutoff_frequency: CF, ) -> FrankaResult<()> { - let motion_generator_cb = |state: &<::Data as RobotData>::State, - duration: &Duration| { + let motion_generator_cb = |state: &::State, duration: &Duration| { motion_generator_callback(&(state.clone().into()), duration) }; - let mut control_cb = |state: &<::Data as RobotData>::State, - duration: &Duration| { + let mut control_cb = |state: &::State, duration: &Duration| { control_callback(&(state.clone().into()), duration) }; self.control_torques_intern( @@ -959,12 +952,10 @@ where limit_rate: L, cutoff_frequency: CF, ) -> FrankaResult<()> { - let motion_generator_cb = |state: &<::Data as RobotData>::State, - duration: &Duration| { + let motion_generator_cb = |state: &::State, duration: &Duration| { motion_generator_callback(&(state.clone().into()), duration) }; - let mut control_cb = |state: &<::Data as RobotData>::State, - duration: &Duration| { + let mut control_cb = |state: &::State, duration: &Duration| { control_callback(&(state.clone().into()), duration) }; self.control_torques_intern( From 8147d53864006e5f355d52c8090c8cc55b3fbe22 Mon Sep 17 00:00:00 2001 From: Marco Boneberger Date: Fri, 5 May 2023 17:50:48 +0200 Subject: [PATCH 35/63] get rid of RobotData casts in control_loop.rs --- src/robot/control_loop.rs | 16 ++++++++-------- src/robot/panda.rs | 2 -- 2 files changed, 8 insertions(+), 10 deletions(-) diff --git a/src/robot/control_loop.rs b/src/robot/control_loop.rs index 415173d..a84dea0 100644 --- a/src/robot/control_loop.rs +++ b/src/robot/control_loop.rs @@ -24,13 +24,13 @@ pub struct ControlLoop< 'b, Data: RobotData, T: RobotImplementation, - U: ConvertMotion<::State> + Debug + MotionGeneratorTrait + Finishable, - F: FnMut(&::State, &Duration) -> U, + 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::State>>, + control_callback: Option>, limit_rate: bool, cutoff_frequency: f64, pub motion_id: u32, @@ -41,13 +41,13 @@ impl< 'b, Data: RobotData, T: RobotImplementation, - U: ConvertMotion<::State> + Debug + MotionGeneratorTrait + Finishable, - F: FnMut(&::State, &Duration) -> U, + 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, ::State>, + control_callback: ControlCallback<'b, Data::State>, motion_callback: F, limit_rate: bool, cutoff_frequency: f64, @@ -91,7 +91,7 @@ impl< fn new_intern( robot: &'a mut T, motion_callback: F, - control_callback: Option::State>>, + control_callback: Option>, limit_rate: bool, cutoff_frequency: f64, ) -> FrankaResult { @@ -181,7 +181,7 @@ impl< fn spin_control( &mut self, - robot_state: &::State, + robot_state: &Data::State, time_step: &Duration, command: &mut ControllerCommand, ) -> bool { diff --git a/src/robot/panda.rs b/src/robot/panda.rs index 5beda14..5435a8b 100644 --- a/src/robot/panda.rs +++ b/src/robot/panda.rs @@ -212,8 +212,6 @@ impl PrivateRobot for Panda { } } -pub struct PandaData {} - impl DeviceData for Panda { type CommandHeader = PandaCommandHeader; type CommandEnum = PandaCommandEnum; From 3c4391a0510e63be0050e23bd49c63a8ce1fc37a Mon Sep 17 00:00:00 2001 From: Marco Boneberger Date: Fri, 5 May 2023 17:56:02 +0200 Subject: [PATCH 36/63] remove GripperData type --- src/gripper.rs | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/src/gripper.rs b/src/gripper.rs index 7e6be2a..e4d505b 100644 --- a/src/gripper.rs +++ b/src/gripper.rs @@ -21,7 +21,7 @@ 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, } @@ -645,9 +645,7 @@ mod tests { } } -pub struct GripperData {} - -impl DeviceData for GripperData { +impl DeviceData for Gripper { type CommandHeader = GripperCommandHeader; type CommandEnum = GripperCommandEnum; From 25796806c3737848e12d2de538cdaba330a2006f Mon Sep 17 00:00:00 2001 From: Marco Boneberger Date: Fri, 5 May 2023 17:59:05 +0200 Subject: [PATCH 37/63] make more traits and structs private --- src/robot/control_loop.rs | 2 +- src/robot/control_types.rs | 2 +- src/robot/robot_control.rs | 2 +- src/robot/robot_impl.rs | 2 +- 4 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/robot/control_loop.rs b/src/robot/control_loop.rs index a84dea0..a5f982e 100644 --- a/src/robot/control_loop.rs +++ b/src/robot/control_loop.rs @@ -19,7 +19,7 @@ use crate::robot::types::{ControllerCommand, MotionGeneratorCommand}; use crate::Finishable; type ControlCallback<'b, State> = &'b mut dyn FnMut(&State, &Duration) -> Torques; -pub struct ControlLoop< +pub(crate) struct ControlLoop< 'a, 'b, Data: RobotData, diff --git a/src/robot/control_types.rs b/src/robot/control_types.rs index 85999ef..74f2aea 100644 --- a/src/robot/control_types.rs +++ b/src/robot/control_types.rs @@ -38,7 +38,7 @@ pub enum RealtimeConfig { Ignore, } -pub trait ConvertMotion { +pub(crate) trait ConvertMotion { /// converts the motion type to a MotionGeneratorCommand and applies rate limiting and filtering fn convert_motion( &self, diff --git a/src/robot/robot_control.rs b/src/robot/robot_control.rs index 3f6a705..99a1f36 100644 --- a/src/robot/robot_control.rs +++ b/src/robot/robot_control.rs @@ -6,7 +6,7 @@ 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, diff --git a/src/robot/robot_impl.rs b/src/robot/robot_impl.rs index 3fdd0fd..8e03454 100644 --- a/src/robot/robot_impl.rs +++ b/src/robot/robot_impl.rs @@ -21,7 +21,7 @@ use crate::RobotModel; use std::fs::remove_file; use std::path::Path; -pub trait RobotImplementation: +pub(crate) trait RobotImplementation: RobotControl::State> { fn read_once(&mut self) -> FrankaResult<::State>; From 8087954914df44e418e8e9def5ed6d82c53016a9 Mon Sep 17 00:00:00 2001 From: Marco Boneberger Date: Fri, 19 May 2023 11:33:37 +0200 Subject: [PATCH 38/63] rename RobotWrapper trait to Robot --- examples/cartesian_impedance_control.rs | 6 +- examples/communication_test.rs | 4 +- examples/download_model.rs | 4 +- examples/echo_robot_state.rs | 4 +- examples/generate_cartesian_pose_motion.rs | 4 +- .../generate_cartesian_velocity_motion.rs | 6 +- examples/generate_consecutive_motions.rs | 4 +- examples/generate_elbow_motion.rs | 4 +- examples/generate_joint_position_motion.rs | 4 +- examples/generate_joint_velocity_motion.rs | 4 +- examples/mirror_robot.rs | 8 +- examples/print_joint_poses.rs | 4 +- src/lib.rs | 8 +- src/model.rs | 2 +- src/robot.rs | 983 +++++++++++++++++- src/robot/robot_wrapper.rs | 976 ----------------- 16 files changed, 1012 insertions(+), 1013 deletions(-) delete mode 100644 src/robot/robot_wrapper.rs diff --git a/examples/cartesian_impedance_control.rs b/examples/cartesian_impedance_control.rs index 94b184b..3c90ca5 100644 --- a/examples/cartesian_impedance_control.rs +++ b/examples/cartesian_impedance_control.rs @@ -7,8 +7,8 @@ use clap::Parser; use nalgebra::{Matrix3, Matrix6, Matrix6x1, UnitQuaternion, Vector3, U1, U3}; use franka::{ - array_to_isometry, Fr3, Frame, FrankaResult, Matrix6x7, Panda, RobotModel, RobotState, - RobotWrapper, Torques, Vector7, + array_to_isometry, Fr3, Frame, FrankaResult, Matrix6x7, Panda, Robot, RobotModel, RobotState, + Torques, Vector7, }; /// An example showing a simple cartesian impedance controller without inertia shaping @@ -41,7 +41,7 @@ fn main() -> FrankaResult<()> { } } -fn generate_motion(mut robot: R) -> FrankaResult<()> { +fn generate_motion(mut robot: R) -> FrankaResult<()> { let model = robot.load_model(false)?; let translational_stiffness = 150.; let rotational_stiffness = 10.; diff --git a/examples/communication_test.rs b/examples/communication_test.rs index cd76f6c..5b7a64d 100644 --- a/examples/communication_test.rs +++ b/examples/communication_test.rs @@ -5,7 +5,7 @@ use std::time::Duration; use clap::Parser; -use franka::{Fr3, FrankaResult, MotionFinished, Panda, RobotState, RobotWrapper, Torques}; +use franka::{Fr3, FrankaResult, MotionFinished, Panda, Robot, RobotState, Torques}; /// An example indicating the network performance. /// @@ -34,7 +34,7 @@ fn main() -> FrankaResult<()> { } } -fn generate_motion(mut robot: R) -> FrankaResult<()> { +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 bc5f87d..d16a629 100644 --- a/examples/download_model.rs +++ b/examples/download_model.rs @@ -7,7 +7,7 @@ use std::path::PathBuf; use clap::Parser; use franka::exception::FrankaException::ModelException; -use franka::{Fr3, FrankaResult, Panda, RealtimeConfig, RobotWrapper}; +use franka::{Fr3, FrankaResult, Panda, RealtimeConfig, Robot}; /// Downloads the model for offline usage #[derive(Parser, Debug)] @@ -40,7 +40,7 @@ fn main() -> FrankaResult<()> { } } -fn download_model(mut robot: R, path: PathBuf) -> FrankaResult<()> { +fn download_model(mut robot: R, path: PathBuf) -> FrankaResult<()> { robot.load_model(true)?; fs::copy("/tmp/model.so", &path).map_err(|_| ModelException { message: "Could copy model to download location".to_string(), diff --git a/examples/echo_robot_state.rs b/examples/echo_robot_state.rs index 4f56310..a23d53f 100644 --- a/examples/echo_robot_state.rs +++ b/examples/echo_robot_state.rs @@ -3,7 +3,7 @@ use clap::Parser; -use franka::{Fr3, FrankaResult, Panda, RobotState, RobotWrapper}; +use franka::{Fr3, FrankaResult, Panda, Robot, RobotState}; /// An example showing how to continuously read the robot state. #[derive(Parser, Debug)] @@ -30,7 +30,7 @@ fn main() -> FrankaResult<()> { } } -fn echo_robot_state(mut robot: R) -> FrankaResult<()> { +fn echo_robot_state(mut robot: R) -> FrankaResult<()> { robot.set_collision_behavior( [0.; 7], [0.; 7], [0.; 7], [0.; 7], [0.; 6], [0.; 6], [0.; 6], [0.; 6], )?; diff --git a/examples/generate_cartesian_pose_motion.rs b/examples/generate_cartesian_pose_motion.rs index 22dfcca..26f25a8 100644 --- a/examples/generate_cartesian_pose_motion.rs +++ b/examples/generate_cartesian_pose_motion.rs @@ -6,7 +6,7 @@ use std::time::Duration; use clap::Parser; -use franka::{CartesianPose, Fr3, FrankaResult, MotionFinished, Panda, RobotState, RobotWrapper}; +use franka::{CartesianPose, Fr3, FrankaResult, MotionFinished, Panda, Robot, RobotState}; /// An example showing how to generate a Cartesian motion. /// @@ -36,7 +36,7 @@ fn main() -> FrankaResult<()> { } } -fn generate_motion(mut robot: R) -> FrankaResult<()> { +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 ab51d0f..f45d1ea 100644 --- a/examples/generate_cartesian_velocity_motion.rs +++ b/examples/generate_cartesian_velocity_motion.rs @@ -6,9 +6,7 @@ use std::time::Duration; use clap::Parser; -use franka::{ - CartesianVelocities, Fr3, FrankaResult, MotionFinished, Panda, RobotState, RobotWrapper, -}; +use franka::{CartesianVelocities, Fr3, FrankaResult, MotionFinished, Panda, Robot, RobotState}; /// An example showing how to generate a Cartesian velocity motion. /// @@ -37,7 +35,7 @@ fn main() -> FrankaResult<()> { } } -fn generate_motion(mut robot: R) -> FrankaResult<()> { +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 58ee01a..d422086 100644 --- a/examples/generate_consecutive_motions.rs +++ b/examples/generate_consecutive_motions.rs @@ -7,7 +7,7 @@ use std::time::Duration; use clap::Parser; use franka::exception::FrankaException; -use franka::{Fr3, FrankaResult, JointVelocities, MotionFinished, Panda, RobotState, RobotWrapper}; +use franka::{Fr3, FrankaResult, JointVelocities, MotionFinished, Panda, Robot, RobotState}; /// An example showing how to execute consecutive motions with error recovery. /// @@ -37,7 +37,7 @@ fn main() -> FrankaResult<()> { } } -fn generate_motion(mut robot: R) -> FrankaResult<()> { +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 a7b8164..f3fb583 100644 --- a/examples/generate_elbow_motion.rs +++ b/examples/generate_elbow_motion.rs @@ -6,7 +6,7 @@ use std::time::Duration; use clap::Parser; -use franka::{CartesianPose, Fr3, FrankaResult, MotionFinished, Panda, RobotState, RobotWrapper}; +use franka::{CartesianPose, Fr3, FrankaResult, MotionFinished, Panda, Robot, RobotState}; /// An example showing how to move the robot's elbow. /// @@ -35,7 +35,7 @@ fn main() -> FrankaResult<()> { } } -fn generate_motion(mut robot: R) -> FrankaResult<()> { +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 8287273..09e708c 100644 --- a/examples/generate_joint_position_motion.rs +++ b/examples/generate_joint_position_motion.rs @@ -6,7 +6,7 @@ use std::time::Duration; use clap::Parser; -use franka::{Fr3, FrankaResult, JointPositions, MotionFinished, Panda, RobotState, RobotWrapper}; +use franka::{Fr3, FrankaResult, JointPositions, MotionFinished, Panda, Robot, RobotState}; /// An example showing how to generate a joint position motion. /// @@ -35,7 +35,7 @@ fn main() -> FrankaResult<()> { } } -fn generate_motion(mut robot: R) -> FrankaResult<()> { +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 c473868..b2adaae 100644 --- a/examples/generate_joint_velocity_motion.rs +++ b/examples/generate_joint_velocity_motion.rs @@ -6,7 +6,7 @@ use std::time::Duration; use clap::Parser; -use franka::{Fr3, FrankaResult, JointVelocities, MotionFinished, Panda, RobotState, RobotWrapper}; +use franka::{Fr3, FrankaResult, JointVelocities, MotionFinished, Panda, Robot, RobotState}; /// An example showing how to generate a joint velocity motion. /// @@ -35,7 +35,7 @@ fn main() -> FrankaResult<()> { } } -fn generate_motion(mut robot: R) -> FrankaResult<()> { +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/mirror_robot.rs b/examples/mirror_robot.rs index bfdc55e..c24829d 100644 --- a/examples/mirror_robot.rs +++ b/examples/mirror_robot.rs @@ -9,8 +9,8 @@ use clap::Parser; use nalgebra::{Matrix3, Matrix6, Matrix6x1, Quaternion, UnitQuaternion, Vector3, U1, U3}; use franka::{ - array_to_isometry, Fr3, Frame, FrankaResult, Matrix6x7, Matrix7, Panda, RobotModel, RobotState, - RobotWrapper, Torques, Vector7, + array_to_isometry, Fr3, Frame, FrankaResult, Matrix6x7, Matrix7, Panda, Robot, RobotModel, + RobotState, Torques, Vector7, }; /// An example where one robot is guided by the user and the other robot acts as a mirror. In this @@ -52,7 +52,7 @@ fn main() -> FrankaResult<()> { } } -fn initialize_and_start_robots( +fn initialize_and_start_robots( args: &CommandLineArguments, robot_user: User, ) -> FrankaResult<()> { @@ -68,7 +68,7 @@ fn initialize_and_start_robots( } } -fn control_robots( +fn control_robots( mut robot_user: User, mut robot_mirror: Mirror, ) -> FrankaResult<()> { diff --git a/examples/print_joint_poses.rs b/examples/print_joint_poses.rs index b86a07e..493a29d 100644 --- a/examples/print_joint_poses.rs +++ b/examples/print_joint_poses.rs @@ -3,7 +3,7 @@ use clap::Parser; use nalgebra::Matrix4; -use franka::{Fr3, Frame, FrankaResult, Panda, RobotModel, RobotWrapper}; +use franka::{Fr3, Frame, FrankaResult, Panda, Robot, RobotModel}; /// An example showing how to use the model library that prints the transformation /// matrix of each joint with respect to the base frame. @@ -31,7 +31,7 @@ fn main() -> FrankaResult<()> { } } -fn generate_motion(mut robot: R) -> FrankaResult<()> { +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/lib.rs b/src/lib.rs index 57ecbb9..b575b0f 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -23,7 +23,7 @@ //!```no_run //! use std::time::Duration; //! use std::f64::consts::PI; -//! use franka::{JointPositions, MotionFinished, RobotWrapper, RobotState, Fr3, FrankaResult}; +//! 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()?; @@ -80,7 +80,7 @@ //!```no_run //! # use std::time::Duration; //! # use std::f64::consts::PI; -//! # use franka::{JointPositions, MotionFinished, RobotState, Fr3, RobotWrapper, FrankaResult}; +//! # use franka::{JointPositions, MotionFinished, RobotState, Fr3, Robot, FrankaResult}; //! # fn main() -> FrankaResult<()> { //! # 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.]; @@ -153,7 +153,7 @@ //!```no_run //! # use std::time::Duration; //! # use std::f64::consts::PI; -//! # use franka::{JointPositions, Fr3, RobotWrapper, RobotState, FrankaResult}; +//! # use franka::{JointPositions, Fr3, Robot, RobotState, FrankaResult}; //! # fn main() -> FrankaResult<()> { //! # let mut robot = Fr3::new("robotik-bs.de", None, None)?; //! # let callback = |state: &RobotState, time_step: &Duration| -> JointPositions {JointPositions::new([0.;7])}; @@ -185,5 +185,5 @@ pub use robot::low_pass_filter::DEFAULT_CUTOFF_FREQUENCY; pub use robot::low_pass_filter::MAX_CUTOFF_FREQUENCY; pub use robot::panda::Panda; pub use robot::robot_state::RobotState; -pub use robot::robot_wrapper::RobotWrapper; +pub use robot::Robot; pub use utils::*; diff --git a/src/model.rs b/src/model.rs index 11f2f66..3fe7c7f 100644 --- a/src/model.rs +++ b/src/model.rs @@ -76,7 +76,7 @@ pub trait RobotModel { /// 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 - /// [`RobotWrapper::load_model`](crate::RobotWrapper::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 diff --git a/src/robot.rs b/src/robot.rs index 45180d2..27a1b13 100644 --- a/src/robot.rs +++ b/src/robot.rs @@ -3,6 +3,22 @@ //! Contains the franka::Robot type. +use crate::exception::create_command_exception; +use crate::robot::robot_control::RobotControl; +use crate::robot::robot_data::{PrivateRobotData, RobotData}; +use crate::robot::robot_impl::RobotImplementation; +use crate::robot::robot_trait::PrivateRobot; +use crate::robot::service_types::{ + SetCartesianImpedanceRequest, SetCollisionBehaviorRequest, SetEeToKRequest, + SetGuidingModeRequest, SetJointImpedanceRequest, SetLoadRequest, SetNeToEeRequest, + StopMoveStatusPanda, +}; +use crate::{ + CartesianPose, CartesianVelocities, ControllerMode, FrankaResult, JointPositions, + JointVelocities, MotionGenerator, RobotModel, RobotState, Torques, MAX_CUTOFF_FREQUENCY, +}; +use std::time::Duration; + mod control_loop; mod control_tools; pub mod control_types; @@ -18,12 +34,973 @@ mod robot_control; pub mod robot_data; mod robot_impl; pub mod robot_state; -pub mod robot_trait; -pub mod robot_wrapper; +mod robot_trait; pub(crate) mod service_types; pub(crate) mod types; pub mod virtual_wall_cuboid; +/// Contains all methods that are available for all robots. +pub trait Robot { + type Model: RobotModel; + + /// 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::{Fr3, RobotState, Robot, FrankaResult}; + /// fn main() -> FrankaResult<()> { + /// let mut robot = Fr3::new("robotik-bs.de",None,None)?; + /// let mut count = 0; + /// robot.read(| robot_state:&RobotState | -> bool { + /// println!("{:?}", robot_state); + /// count += 1; + /// count <= 100 + /// }) + /// } + /// ``` + /// # 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 + /// * [`NetworkException`](`crate::exception::FrankaException::NetworkException`) if the connection is lost, e.g. after a timeout. + fn read bool>(&mut self, read_callback: F) -> 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` 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. + 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<()>; + + /// 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<()>; + + /// Waits for a robot state update and returns it. + /// + /// # 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; + + /// Changes the collision behavior. + /// + /// Set separate torque and force boundaries for acceleration/deceleration and constant velocity + /// movement phases. + /// + /// Forces or torques between lower and upper threshold are shown as contacts in the RobotState. + /// Forces or torques above the upper threshold are registered as collision and cause the robot to + /// stop moving. + /// + /// # Arguments + /// * `lower_torque_thresholds_acceleration` - Contact torque thresholds during + /// acceleration/deceleration for each joint in \[Nm\] + /// * `upper_torque_thresholds_acceleration` - Collision torque thresholds during + /// acceleration/deceleration for each joint in \[Nm\] + /// * `lower_torque_thresholds_nominal` - Contact torque thresholds for each joint in \[Nm\] + /// * `upper_torque_thresholds_nominal` - Collision torque thresholds for each joint in \[Nm\] + /// * `lower_force_thresholds_acceleration` -Contact force thresholds during + /// acceleration/deceleration for (x,y,z,R,P,Y) in \[N\]. + /// * `upper_force_thresholds_acceleration` - Collision force thresholds during + /// acceleration/deceleration for (x,y,z,R,P,Y) in \[N\]. + /// * `lower_force_thresholds_nominal` - Contact force thresholds for (x,y,z,R,P,Y) in \[N\] + /// * `upper_force_thresholds_nominal` - Collision force thresholds for (x,y,z,R,P,Y) in \[N\] + /// # 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. + /// # See also + /// * [`RobotState::cartesian_contact`](`crate::RobotState::cartesian_contact`) + /// * [`RobotState::cartesian_collision`](`crate::RobotState::cartesian_collision`) + /// * [`RobotState::joint_contact`](`crate::RobotState::joint_contact`) + /// * [`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)] + fn set_collision_behavior( + &mut self, + 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<()>; + + /// 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. + /// + /// User-provided torques are not affected by this setting. + /// # Arguments + /// * `K_theta` - Joint impedance values ![K_{\theta}](https://latex.codecogs.com/png.latex?K_{\theta}). + /// # 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_joint_impedance(&mut self, K_theta: [f64; 7]) -> FrankaResult<()>; + + /// Sets the Cartesian impedance for (x, y, z, roll, pitch, yaw) in the internal controller. + /// + /// User-provided torques are not affected by this setting. + /// # Arguments + /// * `K_x` - Cartesian impedance values + /// + /// ![K_x=(x \in \[10,3000\] \frac{N}{m}, y \in \[10,3000\] \frac{N}{m}, z \in \[10,3000\] \frac{N}{m}, R \in \[1,300\] \frac{Nm}{rad}, P \in \[1,300\] \frac{Nm}{rad}, Y \in \[1,300\] \frac{Nm}{rad})](https://latex.codecogs.com/png.latex?K_x=(x&space;\in&space;[10,3000]&space;\frac{N}{m},&space;y&space;\in&space;[10,3000]&space;\frac{N}{m},&space;z&space;\in&space;[10,3000]&space;\frac{N}{m},&space;R&space;\in&space;[1,300]&space;\frac{Nm}{rad},&space;P&space;\in&space;[1,300]&space;\frac{Nm}{rad},&space;Y&space;\in&space;[1,300]&space;\frac{Nm}{rad})) + /// # 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_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. + /// # Note + /// Guiding mode can be enabled by pressing the two opposing buttons near the robot's flange. + /// # Arguments + /// * `guiding_mode` - Unlocked movement in (x, y, z, R, P, Y) in guiding mode. + /// * `elbow` - True if the elbow is free in guiding mode, false otherwise. + /// # 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_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. + /// + /// The transformation matrix is represented as a vectorized 4x4 matrix in column-major format. + /// # Arguments + /// * `EE_T_K` - Vectorized EE-to-K transformation matrix ![^{EE}T_K](https://latex.codecogs.com/png.latex?^{EE}T_K), 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. + /// + /// See [Stiffness frame K](#stiffness-frame-k) for an explanation of the stiffness frame. + #[allow(non_snake_case)] + 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. + /// + /// The transformation matrix is represented as a vectorized 4x4 matrix in column-major format. + /// # Arguments + /// * `NE_T_EE` - Vectorized NE-to-EE transformation matrix ![^{NE}T_{EE}](https://latex.codecogs.com/png.latex?^{NE}T_{EE}), 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. + /// + /// # See also + /// * [`RobotState::NE_T_EE`](`crate::RobotState::NE_T_EE`) + /// * [`RobotState::O_T_EE`](`crate::RobotState::O_T_EE`) + /// * [`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)] + fn set_EE(&mut self, NE_T_EE: [f64; 16]) -> FrankaResult<()>; + + ///Runs automatic error recovery on the robot. + /// + /// Automatic error recovery e.g. resets the robot after a collision occurred. + /// # 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 automatic_error_recovery(&mut self) -> FrankaResult<()>; + + /// Stops all currently running motions. + /// + /// If a control or motion generator loop is running in another thread, it will be preempted + /// with a [`ControlException`](`crate::exception::FrankaException::ControlException`). + /// # 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 stop(&mut self) -> 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` 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. + 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` 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. + 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` 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. + 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. 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. + 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 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. + 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 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. + 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 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. + 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` 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. + fn control_torques_and_cartesian_velocities< + F: FnMut(&RobotState, &Duration) -> CartesianVelocities, + 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<()>; + + /// 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. + fn load_model(&mut self, persistent: bool) -> 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>, +{ + type Model = ::Model; + + 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(()) + } + + fn control_joint_positions< + F: FnMut(&RobotState, &Duration) -> JointPositions, + CM: Into>, + L: Into>, + CF: Into>, + >( + &mut self, + 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) + }; + ::control_motion_intern( + self, + cb, + controller_mode.into(), + limit_rate.into(), + cutoff_frequency.into(), + ) + } + + 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 read_once(&mut self) -> FrankaResult { + match ::get_rob_mut(self).read_once() { + Ok(state) => Ok(state.into()), + Err(e) => Err(e), + } + } + + #[allow(clippy::too_many_arguments)] + fn set_collision_behavior( + &mut self, + 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 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) + } + + 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], + )?; + self.set_joint_impedance([3000., 3000., 3000., 2500., 2500., 2000., 2000.])?; + self.set_cartesian_impedance([3000., 3000., 3000., 300., 300., 300.])?; + Ok(()) + } + + #[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) + } + + #[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) + } + + #[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 control_joint_velocities< + F: FnMut(&RobotState, &Duration) -> JointVelocities, + CM: Into>, + L: Into>, + CF: Into>, + >( + &mut self, + 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) // todo make this without cloning + }; + ::control_motion_intern( + self, + cb, + controller_mode.into(), + limit_rate.into(), + cutoff_frequency.into(), + ) + } + + fn control_cartesian_pose< + F: FnMut(&RobotState, &Duration) -> CartesianPose, + CM: Into>, + L: Into>, + CF: Into>, + >( + &mut self, + 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) + }; + ::control_motion_intern( + self, + cb, + controller_mode.into(), + limit_rate.into(), + cutoff_frequency.into(), + ) + } + + fn control_cartesian_velocities< + F: FnMut(&RobotState, &Duration) -> CartesianVelocities, + CM: Into>, + L: Into>, + CF: Into>, + >( + &mut self, + 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( + cb, + controller_mode.into(), + limit_rate.into(), + cutoff_frequency.into(), + ) + } + + fn control_torques< + T: FnMut(&RobotState, &Duration) -> Torques, + L: Into>, + CF: Into>, + >( + &mut self, + mut control_callback: T, + 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 cb, + limit_rate.into(), + cutoff_frequency.into(), + ) + } + + 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, + 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_cb, + &mut control_cb, + limit_rate.into(), + cutoff_frequency.into(), + ) + } + + fn control_torques_and_joint_positions< + F: FnMut(&RobotState, &Duration) -> JointPositions, + 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_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_cb, + &mut control_cb, + limit_rate.into(), + cutoff_frequency.into(), + ) + } + + 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, + 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_cb, + &mut control_cb, + limit_rate.into(), + cutoff_frequency.into(), + ) + } + + 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_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_cb, + &mut control_cb, + limit_rate.into(), + cutoff_frequency.into(), + ) + } + + fn load_model(&mut self, persistent: bool) -> FrankaResult { + ::get_rob_mut(self).load_model(persistent) + } + + fn server_version(&self) -> u16 { + ::get_rob(self).server_version() + } +} + #[cfg(test)] mod tests { use mockall::{automock, predicate::*}; @@ -44,7 +1021,7 @@ mod tests { SetterResponseFr3, COMMAND_PORT, FR3_VERSION, }; use crate::robot::types::PandaStateIntern; - use crate::{Fr3, RobotWrapper}; + use crate::{Fr3, Robot}; use crate::{FrankaResult, JointPositions, MotionFinished, Panda, RealtimeConfig, RobotState}; use bincode::{deserialize, serialize, serialized_size}; use std::iter::FromIterator; diff --git a/src/robot/robot_wrapper.rs b/src/robot/robot_wrapper.rs deleted file mode 100644 index 0ea957f..0000000 --- a/src/robot/robot_wrapper.rs +++ /dev/null @@ -1,976 +0,0 @@ -use crate::exception::create_command_exception; -use crate::robot::robot_control::RobotControl; -use crate::robot::robot_data::{PrivateRobotData, RobotData}; -use crate::robot::robot_impl::RobotImplementation; -use crate::robot::robot_trait::PrivateRobot; -use crate::robot::service_types::{ - SetCartesianImpedanceRequest, SetCollisionBehaviorRequest, SetEeToKRequest, - SetGuidingModeRequest, SetJointImpedanceRequest, SetLoadRequest, SetNeToEeRequest, - StopMoveStatusPanda, -}; -use crate::{ - CartesianPose, CartesianVelocities, ControllerMode, FrankaResult, JointPositions, - JointVelocities, MotionGenerator, RobotModel, RobotState, Torques, MAX_CUTOFF_FREQUENCY, -}; -use std::time::Duration; - -pub trait RobotWrapper { - type Model: RobotModel; - - /// 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::{Fr3, RobotState, RobotWrapper, FrankaResult}; - /// fn main() -> FrankaResult<()> { - /// let mut robot = Fr3::new("robotik-bs.de",None,None)?; - /// let mut count = 0; - /// robot.read(| robot_state:&RobotState | -> bool { - /// println!("{:?}", robot_state); - /// count += 1; - /// count <= 100 - /// }) - /// } - /// ``` - /// # 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 - /// * [`NetworkException`](`crate::exception::FrankaException::NetworkException`) if the connection is lost, e.g. after a timeout. - fn read bool>(&mut self, read_callback: F) -> 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` 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. - 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<()>; - - /// 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<()>; - - /// Waits for a robot state update and returns it. - /// - /// # 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; - - /// Changes the collision behavior. - /// - /// Set separate torque and force boundaries for acceleration/deceleration and constant velocity - /// movement phases. - /// - /// Forces or torques between lower and upper threshold are shown as contacts in the RobotState. - /// Forces or torques above the upper threshold are registered as collision and cause the robot to - /// stop moving. - /// - /// # Arguments - /// * `lower_torque_thresholds_acceleration` - Contact torque thresholds during - /// acceleration/deceleration for each joint in \[Nm\] - /// * `upper_torque_thresholds_acceleration` - Collision torque thresholds during - /// acceleration/deceleration for each joint in \[Nm\] - /// * `lower_torque_thresholds_nominal` - Contact torque thresholds for each joint in \[Nm\] - /// * `upper_torque_thresholds_nominal` - Collision torque thresholds for each joint in \[Nm\] - /// * `lower_force_thresholds_acceleration` -Contact force thresholds during - /// acceleration/deceleration for (x,y,z,R,P,Y) in \[N\]. - /// * `upper_force_thresholds_acceleration` - Collision force thresholds during - /// acceleration/deceleration for (x,y,z,R,P,Y) in \[N\]. - /// * `lower_force_thresholds_nominal` - Contact force thresholds for (x,y,z,R,P,Y) in \[N\] - /// * `upper_force_thresholds_nominal` - Collision force thresholds for (x,y,z,R,P,Y) in \[N\] - /// # 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. - /// # See also - /// * [`RobotState::cartesian_contact`](`crate::RobotState::cartesian_contact`) - /// * [`RobotState::cartesian_collision`](`crate::RobotState::cartesian_collision`) - /// * [`RobotState::joint_contact`](`crate::RobotState::joint_contact`) - /// * [`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)] - fn set_collision_behavior( - &mut self, - 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<()>; - - /// 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. - /// - /// User-provided torques are not affected by this setting. - /// # Arguments - /// * `K_theta` - Joint impedance values ![K_{\theta}](https://latex.codecogs.com/png.latex?K_{\theta}). - /// # 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_joint_impedance(&mut self, K_theta: [f64; 7]) -> FrankaResult<()>; - - /// Sets the Cartesian impedance for (x, y, z, roll, pitch, yaw) in the internal controller. - /// - /// User-provided torques are not affected by this setting. - /// # Arguments - /// * `K_x` - Cartesian impedance values - /// - /// ![K_x=(x \in \[10,3000\] \frac{N}{m}, y \in \[10,3000\] \frac{N}{m}, z \in \[10,3000\] \frac{N}{m}, R \in \[1,300\] \frac{Nm}{rad}, P \in \[1,300\] \frac{Nm}{rad}, Y \in \[1,300\] \frac{Nm}{rad})](https://latex.codecogs.com/png.latex?K_x=(x&space;\in&space;[10,3000]&space;\frac{N}{m},&space;y&space;\in&space;[10,3000]&space;\frac{N}{m},&space;z&space;\in&space;[10,3000]&space;\frac{N}{m},&space;R&space;\in&space;[1,300]&space;\frac{Nm}{rad},&space;P&space;\in&space;[1,300]&space;\frac{Nm}{rad},&space;Y&space;\in&space;[1,300]&space;\frac{Nm}{rad})) - /// # 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_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. - /// # Note - /// Guiding mode can be enabled by pressing the two opposing buttons near the robot's flange. - /// # Arguments - /// * `guiding_mode` - Unlocked movement in (x, y, z, R, P, Y) in guiding mode. - /// * `elbow` - True if the elbow is free in guiding mode, false otherwise. - /// # 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_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. - /// - /// The transformation matrix is represented as a vectorized 4x4 matrix in column-major format. - /// # Arguments - /// * `EE_T_K` - Vectorized EE-to-K transformation matrix ![^{EE}T_K](https://latex.codecogs.com/png.latex?^{EE}T_K), 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. - /// - /// See [Stiffness frame K](#stiffness-frame-k) for an explanation of the stiffness frame. - #[allow(non_snake_case)] - 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. - /// - /// The transformation matrix is represented as a vectorized 4x4 matrix in column-major format. - /// # Arguments - /// * `NE_T_EE` - Vectorized NE-to-EE transformation matrix ![^{NE}T_{EE}](https://latex.codecogs.com/png.latex?^{NE}T_{EE}), 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. - /// - /// # See also - /// * [`RobotState::NE_T_EE`](`crate::RobotState::NE_T_EE`) - /// * [`RobotState::O_T_EE`](`crate::RobotState::O_T_EE`) - /// * [`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)] - fn set_EE(&mut self, NE_T_EE: [f64; 16]) -> FrankaResult<()>; - - ///Runs automatic error recovery on the robot. - /// - /// Automatic error recovery e.g. resets the robot after a collision occurred. - /// # 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 automatic_error_recovery(&mut self) -> FrankaResult<()>; - - /// Stops all currently running motions. - /// - /// If a control or motion generator loop is running in another thread, it will be preempted - /// with a [`ControlException`](`crate::exception::FrankaException::ControlException`). - /// # 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 stop(&mut self) -> 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` 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. - 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` 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. - 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` 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. - 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. 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. - 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 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. - 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 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. - 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 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. - 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` 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. - fn control_torques_and_cartesian_velocities< - F: FnMut(&RobotState, &Duration) -> CartesianVelocities, - 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<()>; - - /// 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. - fn load_model(&mut self, persistent: bool) -> FrankaResult; - - /// Returns the software version reported by the connected server. - /// - /// # Return - /// Software version of the connected server. - fn server_version(&self) -> u16; -} - -impl RobotWrapper for R -where - RobotState: From<::State>, - CartesianPose: crate::ConvertMotion<::State>, - JointVelocities: crate::ConvertMotion<::State>, - JointPositions: crate::ConvertMotion<::State>, - CartesianVelocities: crate::ConvertMotion<::State>, -{ - type Model = ::Model; - - 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(()) - } - - fn control_joint_positions< - F: FnMut(&RobotState, &Duration) -> JointPositions, - CM: Into>, - L: Into>, - CF: Into>, - >( - &mut self, - 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) - }; - ::control_motion_intern( - self, - cb, - controller_mode.into(), - limit_rate.into(), - cutoff_frequency.into(), - ) - } - - 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 read_once(&mut self) -> FrankaResult { - match ::get_rob_mut(self).read_once() { - Ok(state) => Ok(state.into()), - Err(e) => Err(e), - } - } - - #[allow(clippy::too_many_arguments)] - fn set_collision_behavior( - &mut self, - 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 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) - } - - 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], - )?; - self.set_joint_impedance([3000., 3000., 3000., 2500., 2500., 2000., 2000.])?; - self.set_cartesian_impedance([3000., 3000., 3000., 300., 300., 300.])?; - Ok(()) - } - - #[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) - } - - #[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) - } - - #[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 control_joint_velocities< - F: FnMut(&RobotState, &Duration) -> JointVelocities, - CM: Into>, - L: Into>, - CF: Into>, - >( - &mut self, - 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) // todo make this without cloning - }; - ::control_motion_intern( - self, - cb, - controller_mode.into(), - limit_rate.into(), - cutoff_frequency.into(), - ) - } - - fn control_cartesian_pose< - F: FnMut(&RobotState, &Duration) -> CartesianPose, - CM: Into>, - L: Into>, - CF: Into>, - >( - &mut self, - 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) - }; - ::control_motion_intern( - self, - cb, - controller_mode.into(), - limit_rate.into(), - cutoff_frequency.into(), - ) - } - - fn control_cartesian_velocities< - F: FnMut(&RobotState, &Duration) -> CartesianVelocities, - CM: Into>, - L: Into>, - CF: Into>, - >( - &mut self, - 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( - cb, - controller_mode.into(), - limit_rate.into(), - cutoff_frequency.into(), - ) - } - - fn control_torques< - T: FnMut(&RobotState, &Duration) -> Torques, - L: Into>, - CF: Into>, - >( - &mut self, - mut control_callback: T, - 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 cb, - limit_rate.into(), - cutoff_frequency.into(), - ) - } - - 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, - 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_cb, - &mut control_cb, - limit_rate.into(), - cutoff_frequency.into(), - ) - } - - fn control_torques_and_joint_positions< - F: FnMut(&RobotState, &Duration) -> JointPositions, - 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_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_cb, - &mut control_cb, - limit_rate.into(), - cutoff_frequency.into(), - ) - } - - 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, - 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_cb, - &mut control_cb, - limit_rate.into(), - cutoff_frequency.into(), - ) - } - - 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_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_cb, - &mut control_cb, - limit_rate.into(), - cutoff_frequency.into(), - ) - } - - fn load_model(&mut self, persistent: bool) -> FrankaResult { - ::get_rob_mut(self).load_model(persistent) - } - - fn server_version(&self) -> u16 { - ::get_rob(self).server_version() - } -} From 35bac93d4cdbda526e776a7709b416bc21619777 Mon Sep 17 00:00:00 2001 From: Marco Boneberger Date: Fri, 19 May 2023 14:34:17 +0200 Subject: [PATCH 39/63] rename robot_trait.rs to private_robot.rs --- src/robot.rs | 4 ++-- src/robot/fr3.rs | 2 +- src/robot/panda.rs | 2 +- src/robot/{robot_trait.rs => private_robot.rs} | 0 src/robot/robot_impl.rs | 7 +------ 5 files changed, 5 insertions(+), 10 deletions(-) rename src/robot/{robot_trait.rs => private_robot.rs} (100%) diff --git a/src/robot.rs b/src/robot.rs index 27a1b13..34a41f7 100644 --- a/src/robot.rs +++ b/src/robot.rs @@ -4,10 +4,10 @@ //! Contains the franka::Robot type. use crate::exception::create_command_exception; +use crate::robot::private_robot::PrivateRobot; use crate::robot::robot_control::RobotControl; use crate::robot::robot_data::{PrivateRobotData, RobotData}; use crate::robot::robot_impl::RobotImplementation; -use crate::robot::robot_trait::PrivateRobot; use crate::robot::service_types::{ SetCartesianImpedanceRequest, SetCollisionBehaviorRequest, SetEeToKRequest, SetGuidingModeRequest, SetJointImpedanceRequest, SetLoadRequest, SetNeToEeRequest, @@ -29,12 +29,12 @@ pub mod logger; pub mod low_pass_filter; mod motion_generator_traits; pub mod panda; +mod private_robot; mod rate_limiting; mod robot_control; pub mod robot_data; mod robot_impl; pub mod robot_state; -mod robot_trait; pub(crate) mod service_types; pub(crate) mod types; pub mod virtual_wall_cuboid; diff --git a/src/robot/fr3.rs b/src/robot/fr3.rs index 6f85262..b27827d 100644 --- a/src/robot/fr3.rs +++ b/src/robot/fr3.rs @@ -3,9 +3,9 @@ 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::robot_data::{PrivateRobotData, RobotData}; use crate::robot::robot_impl::RobotImplGeneric; -use crate::robot::robot_trait::PrivateRobot; use crate::robot::service_types; use crate::robot::service_types::{ AutomaticErrorRecoveryStatusFr3, ConnectRequest, ConnectRequestWithFr3Header, Fr3CommandEnum, diff --git a/src/robot/panda.rs b/src/robot/panda.rs index 5435a8b..f5eeed6 100644 --- a/src/robot/panda.rs +++ b/src/robot/panda.rs @@ -3,9 +3,9 @@ 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::robot_data::{PrivateRobotData, RobotData}; use crate::robot::robot_impl::RobotImplGeneric; -use crate::robot::robot_trait::PrivateRobot; use crate::robot::service_types; use crate::robot::service_types::{ AutomaticErrorRecoveryStatusPanda, ConnectRequest, ConnectRequestWithPandaHeader, diff --git a/src/robot/robot_trait.rs b/src/robot/private_robot.rs similarity index 100% rename from src/robot/robot_trait.rs rename to src/robot/private_robot.rs diff --git a/src/robot/robot_impl.rs b/src/robot/robot_impl.rs index 8e03454..bdda70f 100644 --- a/src/robot/robot_impl.rs +++ b/src/robot/robot_impl.rs @@ -217,8 +217,6 @@ impl RobotControl for RobotImplGeneric { } impl RobotImplementation for RobotImplGeneric { - // type Data = Data; - fn read_once(&mut self) -> FrankaResult { while self.network.udp_receive::().is_some() {} Ok(Data::State::from(self.receive_robot_state()?)) @@ -292,10 +290,7 @@ impl RobotImplGeneric { self.controller_mode = robot_state.get_controller_mode(); self.message_id = robot_state.get_message_id(); } - // pub fn read_once(&mut self) -> FrankaResult { - // while self.network.udp_receive::().is_some() {} - // Ok(Data::State::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::(); From e05d4e99f5c68856c0c55c9a3e113b931725f238 Mon Sep 17 00:00:00 2001 From: Marco Boneberger Date: Fri, 19 May 2023 14:43:51 +0200 Subject: [PATCH 40/63] fix intradoc links --- src/robot.rs | 18 ------------------ src/robot/panda.rs | 4 ++-- 2 files changed, 2 insertions(+), 20 deletions(-) diff --git a/src/robot.rs b/src/robot.rs index 34a41f7..229396a 100644 --- a/src/robot.rs +++ b/src/robot.rs @@ -89,8 +89,6 @@ pub trait Robot { /// * [`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. fn control_joint_positions< F: FnMut(&RobotState, &Duration) -> JointPositions, CM: Into>, @@ -299,8 +297,6 @@ pub trait Robot { /// * [`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. fn control_joint_velocities< F: FnMut(&RobotState, &Duration) -> JointVelocities, CM: Into>, @@ -336,8 +332,6 @@ pub trait Robot { /// * [`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. fn control_cartesian_pose< F: FnMut(&RobotState, &Duration) -> CartesianPose, CM: Into>, @@ -373,8 +367,6 @@ pub trait Robot { /// * [`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. fn control_cartesian_velocities< F: FnMut(&RobotState, &Duration) -> CartesianVelocities, CM: Into>, @@ -412,8 +404,6 @@ pub trait Robot { /// 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. fn control_torques< T: FnMut(&RobotState, &Duration) -> Torques, L: Into>, @@ -447,8 +437,6 @@ pub trait Robot { /// * [`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. fn control_torques_and_joint_velocities< F: FnMut(&RobotState, &Duration) -> JointVelocities, T: FnMut(&RobotState, &Duration) -> Torques, @@ -484,8 +472,6 @@ pub trait Robot { /// * [`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. fn control_torques_and_joint_positions< F: FnMut(&RobotState, &Duration) -> JointPositions, T: FnMut(&RobotState, &Duration) -> Torques, @@ -521,8 +507,6 @@ pub trait Robot { /// * [`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. fn control_torques_and_cartesian_pose< F: FnMut(&RobotState, &Duration) -> CartesianPose, T: FnMut(&RobotState, &Duration) -> Torques, @@ -558,8 +542,6 @@ pub trait Robot { /// * [`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. fn control_torques_and_cartesian_velocities< F: FnMut(&RobotState, &Duration) -> CartesianVelocities, T: FnMut(&RobotState, &Duration) -> Torques, diff --git a/src/robot/panda.rs b/src/robot/panda.rs index f5eeed6..9e7b8d4 100644 --- a/src/robot/panda.rs +++ b/src/robot/panda.rs @@ -30,11 +30,11 @@ use std::mem::size_of; /// # 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`](`crate::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`](``crate::Robot::set_K`). /// /// /// # Motion generation and joint-level torque commands From 47cf74e18cad31a505e8b66bb5ce3c54bf4f9dff Mon Sep 17 00:00:00 2001 From: Marco Boneberger Date: Fri, 19 May 2023 15:04:29 +0200 Subject: [PATCH 41/63] disable rate-limiter by default for FR3 --- src/robot.rs | 48 +++++++++++++++++++++++++------------- src/robot/panda.rs | 3 +-- src/robot/private_robot.rs | 4 ++-- src/robot/robot_data.rs | 1 + 4 files changed, 36 insertions(+), 20 deletions(-) diff --git a/src/robot.rs b/src/robot.rs index 229396a..e19e7e9 100644 --- a/src/robot.rs +++ b/src/robot.rs @@ -76,8 +76,10 @@ pub trait Robot { /// * `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! + /// * `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`](`RobotData::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. @@ -285,8 +287,10 @@ pub trait Robot { /// * `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! + /// * `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`](`RobotData::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. @@ -319,8 +323,10 @@ pub trait Robot { /// * `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! + /// * `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`](`RobotData::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. @@ -354,8 +360,10 @@ pub trait Robot { /// * `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! + /// * `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`](`RobotData::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. @@ -425,8 +433,10 @@ pub trait Robot { /// 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! + /// * `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`](`RobotData::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. @@ -460,8 +470,10 @@ pub trait Robot { /// 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! + /// * `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`](`RobotData::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. @@ -495,8 +507,10 @@ pub trait Robot { /// 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! + /// * `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`](`RobotData::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. @@ -530,8 +544,10 @@ pub trait Robot { /// 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! + /// * `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`](`RobotData::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. diff --git a/src/robot/panda.rs b/src/robot/panda.rs index 9e7b8d4..9d5fdad 100644 --- a/src/robot/panda.rs +++ b/src/robot/panda.rs @@ -196,9 +196,7 @@ impl Panda { } impl PrivateRobot for Panda { - // type Rob = ; type Rob = RobotImplGeneric; - // type PrivateData = Panda; fn get_rob_mut(&mut self) -> &mut Self::Rob { &mut self.0 @@ -231,6 +229,7 @@ impl DeviceData for Panda { } impl RobotData for Panda { + const RATE_LIMITING_ON_PER_DEFAULT: bool = true; type Model = PandaModel; type StateIntern = PandaStateIntern; type State = RobotState; diff --git a/src/robot/private_robot.rs b/src/robot/private_robot.rs index 41d5533..278e86f 100644 --- a/src/robot/private_robot.rs +++ b/src/robot/private_robot.rs @@ -26,7 +26,7 @@ pub(crate) trait PrivateRobot: PrivateRobotData + Sized { cutoff_frequency: Option, ) -> FrankaResult<()> { let controller_mode = controller_mode.unwrap_or(ControllerMode::JointImpedance); - let limit_rate = limit_rate.unwrap_or(true); + 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(), @@ -48,7 +48,7 @@ pub(crate) trait PrivateRobot: PrivateRobotData + Sized { limit_rate: Option, cutoff_frequency: Option, ) -> FrankaResult<()> { - let limit_rate = limit_rate.unwrap_or(true); + 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(), diff --git a/src/robot/robot_data.rs b/src/robot/robot_data.rs index bea80fa..9972b2d 100644 --- a/src/robot/robot_data.rs +++ b/src/robot/robot_data.rs @@ -16,6 +16,7 @@ use serde::Serialize; use std::fmt::Debug; pub trait RobotData { + const RATE_LIMITING_ON_PER_DEFAULT: bool = false; type Model: RobotModel; type StateIntern: Debug + DeserializeOwned + Serialize + AbstractRobotStateIntern + 'static; type State: AbstractRobotState + From + From; From 08d3fa834012be6ba3f49326633fe9432818dea1 Mon Sep 17 00:00:00 2001 From: Marco Boneberger Date: Fri, 19 May 2023 15:52:32 +0200 Subject: [PATCH 42/63] support to models for different architectures on linux --- README.md | 3 -- src/model/library_downloader.rs | 53 +++++++++++++++++++++------------ src/robot/fr3.rs | 1 + src/robot/panda.rs | 1 + src/robot/robot_data.rs | 1 + src/robot/robot_impl.rs | 11 ++++++- 6 files changed, 47 insertions(+), 23 deletions(-) diff --git a/README.md b/README.md index c140ee8..47bc4d9 100644 --- a/README.md +++ b/README.md @@ -24,9 +24,6 @@ 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) diff --git a/src/model/library_downloader.rs b/src/model/library_downloader.rs index c0b6281..e38ea91 100644 --- a/src/model/library_downloader.rs +++ b/src/model/library_downloader.rs @@ -27,30 +27,45 @@ impl LibraryDownloader for LibraryDownloaderGeneric, download_path: &Path) -> FrankaResult<()> { - if cfg!(all(target_os = "linux", target_arch = "x86_64")) { - let request = LoadModelLibraryRequest { + let request = if cfg!(all(target_os = "linux", target_arch = "x86_64")) { + LoadModelLibraryRequest { architecture: LoadModelLibraryArchitecture::X64, system: LoadModelLibrarySystem::Linux, - }; - 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(()) + } + } 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(), - }) - } + }); + }; + 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/robot/fr3.rs b/src/robot/fr3.rs index b27827d..1ee4734 100644 --- a/src/robot/fr3.rs +++ b/src/robot/fr3.rs @@ -82,6 +82,7 @@ impl DeviceData for Fr3 { } impl PrivateRobotData for Fr3 { + const MODEL_NAME: &'static str = "fr3"; type Header = Fr3CommandHeader; type LoadModelRequestWithHeader = LoadModelLibraryRequestWithFr3Header; type SetCollisionBehaviorRequestWithHeader = SetCollisionBehaviorRequestWithFr3Header; diff --git a/src/robot/panda.rs b/src/robot/panda.rs index 9d5fdad..df90c05 100644 --- a/src/robot/panda.rs +++ b/src/robot/panda.rs @@ -236,6 +236,7 @@ impl RobotData for Panda { } impl PrivateRobotData for Panda { + const MODEL_NAME: &'static str = "panda"; type Header = PandaCommandHeader; type LoadModelRequestWithHeader = LoadModelLibraryRequestWithPandaHeader; type SetCollisionBehaviorRequestWithHeader = SetCollisionBehaviorRequestWithPandaHeader; diff --git a/src/robot/robot_data.rs b/src/robot/robot_data.rs index 9972b2d..55cf501 100644 --- a/src/robot/robot_data.rs +++ b/src/robot/robot_data.rs @@ -23,6 +23,7 @@ pub trait RobotData { } pub(crate) trait PrivateRobotData: DeviceData + RobotData { + const MODEL_NAME: &'static str; type Header: RobotHeader; type LoadModelRequestWithHeader: MessageCommand + Serialize diff --git a/src/robot/robot_impl.rs b/src/robot/robot_impl.rs index bdda70f..ad1f4a5 100644 --- a/src/robot/robot_impl.rs +++ b/src/robot/robot_impl.rs @@ -1,10 +1,12 @@ // Copyright (c) 2021 Marco Boneberger // Licensed under the EUPL-1.2-or-later + 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::logger::Logger; +use std::env::consts::{ARCH, DLL_EXTENSION, OS}; use crate::robot::robot_control::RobotControl; use crate::robot::robot_data::{PrivateRobotData, RobotData}; @@ -222,7 +224,14 @@ impl RobotImplementation for RobotImplGeneric FrankaResult { - let model_file = Path::new("/tmp/model.so"); // TODO when we load from file we need to distinguish between FR3 and panda + 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)?; From 95b9f2957ac8bfd7ffae691b2ebd2b63a282f873 Mon Sep 17 00:00:00 2001 From: Marco Boneberger Date: Fri, 14 Jul 2023 08:58:38 +0200 Subject: [PATCH 43/63] fix commandline options for panda --- examples/communication_test.rs | 2 +- examples/download_model.rs | 2 +- examples/mirror_robot.rs | 8 ++++---- 3 files changed, 6 insertions(+), 6 deletions(-) diff --git a/examples/communication_test.rs b/examples/communication_test.rs index 5b7a64d..d8f44c0 100644 --- a/examples/communication_test.rs +++ b/examples/communication_test.rs @@ -15,7 +15,7 @@ use franka::{Fr3, FrankaResult, MotionFinished, Panda, Robot, RobotState, Torque struct CommandLineArguments { /// IP-Address or hostname of the robot pub franka_ip: String, - // Use this option to run the example on a Panda + /// Use this option to run the example on a Panda #[clap(short, long, action)] pub panda: bool, } diff --git a/examples/download_model.rs b/examples/download_model.rs index d16a629..58c0d5e 100644 --- a/examples/download_model.rs +++ b/examples/download_model.rs @@ -19,7 +19,7 @@ struct CommandLineArguments { /// 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 + /// Use this option to run the example on a Panda #[clap(short, long, action)] pub panda: bool, } diff --git a/examples/mirror_robot.rs b/examples/mirror_robot.rs index c24829d..bdfb2cb 100644 --- a/examples/mirror_robot.rs +++ b/examples/mirror_robot.rs @@ -28,12 +28,12 @@ struct CommandLineArguments { #[clap(long)] pub franka_ip_mirror: String, - /// Use this option to run the example on a Panda - #[clap(short, long, action)] + /// Use this option if the hand-guided robot is a Panda + #[clap(long, action)] pub panda_user: bool, - /// Use this option to run the example on a Panda - #[clap(short, long, action)] + /// Use this option if the mirrored robot is a Panda + #[clap(long, action)] pub panda_mirror: bool, } From 9863d0cdadb43d39a51c45613358a39bad244e3b Mon Sep 17 00:00:00 2001 From: Marco Boneberger Date: Tue, 17 Oct 2023 13:21:39 +0200 Subject: [PATCH 44/63] make rate limiting generic --- src/robot.rs | 18 +- src/robot/control_loop.rs | 6 +- src/robot/control_types.rs | 67 +++--- src/robot/fr3.rs | 2 + src/robot/panda.rs | 3 +- src/robot/private_robot.rs | 5 +- src/robot/rate_limiting.rs | 449 ++++++++++++++++++++++++++++++------- src/robot/robot_data.rs | 3 +- 8 files changed, 427 insertions(+), 126 deletions(-) diff --git a/src/robot.rs b/src/robot.rs index e19e7e9..e1ce3d5 100644 --- a/src/robot.rs +++ b/src/robot.rs @@ -30,7 +30,7 @@ pub mod low_pass_filter; mod motion_generator_traits; pub mod panda; mod private_robot; -mod rate_limiting; +pub mod rate_limiting; mod robot_control; pub mod robot_data; mod robot_impl; @@ -78,7 +78,7 @@ pub trait Robot { /// * `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`](`RobotData::RATE_LIMITING_ON_PER_DEFAULT`). + /// [`RATE_LIMITING_ON_PER_DEFAULT`](`crate::robot::rate_limiting::RateLimiterParameters::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. @@ -289,7 +289,7 @@ pub trait Robot { /// * `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`](`RobotData::RATE_LIMITING_ON_PER_DEFAULT`). + /// [`RATE_LIMITING_ON_PER_DEFAULT`](`crate::robot::rate_limiting::RateLimiterParameters::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. @@ -325,7 +325,7 @@ pub trait Robot { /// * `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`](`RobotData::RATE_LIMITING_ON_PER_DEFAULT`). + /// [`RATE_LIMITING_ON_PER_DEFAULT`](`crate::robot::rate_limiting::RateLimiterParameters::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. @@ -362,7 +362,7 @@ pub trait Robot { /// * `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`](`RobotData::RATE_LIMITING_ON_PER_DEFAULT`). + /// [`RATE_LIMITING_ON_PER_DEFAULT`](`crate::robot::rate_limiting::RateLimiterParameters::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. @@ -435,7 +435,7 @@ pub trait Robot { /// 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`](`RobotData::RATE_LIMITING_ON_PER_DEFAULT`). + /// [`RATE_LIMITING_ON_PER_DEFAULT`](`crate::robot::rate_limiting::RateLimiterParameters::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. @@ -472,7 +472,7 @@ pub trait Robot { /// 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`](`RobotData::RATE_LIMITING_ON_PER_DEFAULT`). + /// [`RATE_LIMITING_ON_PER_DEFAULT`](`crate::robot::rate_limiting::RateLimiterParameters::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. @@ -509,7 +509,7 @@ pub trait Robot { /// 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`](`RobotData::RATE_LIMITING_ON_PER_DEFAULT`). + /// [`RATE_LIMITING_ON_PER_DEFAULT`](`crate::robot::rate_limiting::RateLimiterParameters::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. @@ -546,7 +546,7 @@ pub trait Robot { /// 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`](`RobotData::RATE_LIMITING_ON_PER_DEFAULT`). + /// [`RATE_LIMITING_ON_PER_DEFAULT`](`crate::robot::rate_limiting::RateLimiterParameters::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. diff --git a/src/robot/control_loop.rs b/src/robot/control_loop.rs index a5f982e..934a092 100644 --- a/src/robot/control_loop.rs +++ b/src/robot/control_loop.rs @@ -10,7 +10,7 @@ use crate::robot::control_tools::{ 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::rate_limiting::{limit_rate_torques, DELTA_T, RateLimiterParameters}; use crate::robot::robot_data::RobotData; use crate::robot::robot_impl::RobotImplementation; use crate::robot::robot_state::AbstractRobotState; @@ -199,7 +199,7 @@ impl< } if self.limit_rate { control_output.tau_J = limit_rate_torques( - &MAX_TORQUE_RATE, + &Data::RateLimiterParameters::MAX_TORQUE_RATE, &control_output.tau_J, &robot_state.get_tau_J_d(), ); @@ -215,7 +215,7 @@ impl< 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 74f2aea..1d05cf1 100644 --- a/src/robot/control_types.rs +++ b/src/robot/control_types.rs @@ -13,10 +13,7 @@ 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, RateLimiterParameters, DELTA_T, }; use crate::robot::robot_state::{AbstractRobotState, RobotState}; use crate::robot::service_types::MoveMotionGeneratorMode; @@ -40,7 +37,7 @@ pub enum RealtimeConfig { pub(crate) trait ConvertMotion { /// converts the motion type to a MotionGeneratorCommand and applies rate limiting and filtering - fn convert_motion( + fn convert_motion( &self, robot_state: &State, command: &mut MotionGeneratorCommand, @@ -143,7 +140,7 @@ impl Finishable for JointPositions { } } impl ConvertMotion for JointPositions { - fn convert_motion( + fn convert_motion( &self, robot_state: &RobotState, command: &mut MotionGeneratorCommand, @@ -163,9 +160,10 @@ impl ConvertMotion 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, @@ -217,7 +215,7 @@ impl Finishable for JointVelocities { } } impl ConvertMotion for JointVelocities { - fn convert_motion( + fn convert_motion( &self, robot_state: &RobotState, command: &mut MotionGeneratorCommand, @@ -237,9 +235,10 @@ impl ConvertMotion 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, @@ -332,7 +331,7 @@ impl Finishable for CartesianPose { } impl ConvertMotion for CartesianPose { - fn convert_motion( + fn convert_motion( &self, robot_state: &RobotState, command: &mut MotionGeneratorCommand, @@ -351,12 +350,12 @@ impl ConvertMotion 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, @@ -378,9 +377,10 @@ impl ConvertMotion 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], @@ -453,7 +453,7 @@ impl Finishable for CartesianVelocities { } } impl ConvertMotion for CartesianVelocities { - fn convert_motion( + fn convert_motion( &self, robot_state: &RobotState, command: &mut MotionGeneratorCommand, @@ -473,12 +473,12 @@ impl ConvertMotion 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, @@ -502,9 +502,10 @@ impl ConvertMotion 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/fr3.rs b/src/robot/fr3.rs index 1ee4734..4b9e753 100644 --- a/src/robot/fr3.rs +++ b/src/robot/fr3.rs @@ -18,11 +18,13 @@ use crate::robot::service_types::{ use crate::robot::types::Fr3StateIntern; use crate::{Fr3Model, FrankaResult, RealtimeConfig, RobotState}; use std::mem::size_of; +use crate::robot::rate_limiting::Fr3RateLimiter; pub struct Fr3(RobotImplGeneric); impl RobotData for Fr3 { type Model = Fr3Model; + type RateLimiterParameters = Fr3RateLimiter; type StateIntern = Fr3StateIntern; type State = RobotState; } diff --git a/src/robot/panda.rs b/src/robot/panda.rs index df90c05..ae88814 100644 --- a/src/robot/panda.rs +++ b/src/robot/panda.rs @@ -22,6 +22,7 @@ use crate::robot::types::PandaStateIntern; use crate::robot::virtual_wall_cuboid::VirtualWallCuboid; use crate::{FrankaResult, PandaModel, RealtimeConfig, RobotState}; use std::mem::size_of; +use crate::robot::rate_limiting::PandaRateLimiter; /// Maintains a network connection to the robot, provides the current robot state, gives access to /// the model library and allows to control the robot. @@ -229,8 +230,8 @@ impl DeviceData for Panda { } impl RobotData for Panda { - const RATE_LIMITING_ON_PER_DEFAULT: bool = true; type Model = PandaModel; + type RateLimiterParameters = PandaRateLimiter; type StateIntern = PandaStateIntern; type State = RobotState; } diff --git a/src/robot/private_robot.rs b/src/robot/private_robot.rs index 278e86f..0ffac7a 100644 --- a/src/robot/private_robot.rs +++ b/src/robot/private_robot.rs @@ -8,6 +8,7 @@ use crate::{ }; use std::fmt::Debug; use std::time::Duration; +use crate::robot::rate_limiting::RateLimiterParameters; pub(crate) trait PrivateRobot: PrivateRobotData + Sized { type Rob: RobotImplementation; @@ -26,7 +27,7 @@ pub(crate) trait PrivateRobot: PrivateRobotData + Sized { 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 limit_rate = limit_rate.unwrap_or(::RateLimiterParameters::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(), @@ -48,7 +49,7 @@ pub(crate) trait PrivateRobot: PrivateRobotData + Sized { limit_rate: Option, cutoff_frequency: Option, ) -> FrankaResult<()> { - let limit_rate = limit_rate.unwrap_or(::RATE_LIMITING_ON_PER_DEFAULT); + let limit_rate = limit_rate.unwrap_or(::RateLimiterParameters::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(), diff --git a/src/robot/rate_limiting.rs b/src/robot/rate_limiting.rs index 10594bf..84b4680 100644 --- a/src/robot/rate_limiting.rs +++ b/src/robot/rate_limiting.rs @@ -12,76 +12,320 @@ use nalgebra::{ 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; + +pub trait RateLimiterParameters { + 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]; +} + +/// A constant velocity rate limiter for the Panda robot. +/// This implementation is based on the rate limiter in the original libfranka library (version 0.9.2). +/// In this rate limiter the position is completely ignored for calculating the maximum and minimum +/// joint velocities. +pub struct PandaRateLimiter {} + +impl PandaRateLimiter { + /// 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 RateLimiterParameters for PandaRateLimiter { + 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 + } +} + +/// 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). +pub struct Fr3RateLimiter {} +impl Fr3RateLimiter { + /// 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 RateLimiterParameters for Fr3RateLimiter { + 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], + ), + ] + } +} /// 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. @@ -94,6 +338,7 @@ pub static MAX_ELBOW_VELOCITY: f64 = /// Rate-limited vector of desired joint positions. 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 +353,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 +370,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. @@ -135,6 +382,7 @@ pub fn limit_rate_joint_positions( /// Rate-limited desired joint position. pub fn limit_rate_position( max_velocity: f64, + min_velocity: f64, max_acceleration: f64, max_jerk: f64, commanded_position: f64, @@ -146,6 +394,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 +408,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 +420,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 +438,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 +483,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 +495,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 +509,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], @@ -461,9 +715,8 @@ mod tests { 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, + limit_rate_cartesian_pose, Fr3RateLimiter, PandaRateLimiter, RateLimiterParameters, + DELTA_T, LIMIT_EPS, }; use crate::utils::array_to_isometry; @@ -637,12 +890,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, + PandaRateLimiter::MAX_TRANSLATIONAL_VELOCITY, + PandaRateLimiter::MAX_TRANSLATIONAL_ACCELERATION, + PandaRateLimiter::MAX_TRANSLATIONAL_JERK, + PandaRateLimiter::MAX_ROTATIONAL_VELOCITY, + PandaRateLimiter::MAX_ROTATIONAL_ACCELERATION, + PandaRateLimiter::MAX_ROTATIONAL_JERK, &O_T_EE_c, &last_O_T_EE_c, &O_dP_EE_c, @@ -674,12 +927,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, + PandaRateLimiter::MAX_TRANSLATIONAL_VELOCITY, + PandaRateLimiter::MAX_TRANSLATIONAL_ACCELERATION, + PandaRateLimiter::MAX_TRANSLATIONAL_JERK, + PandaRateLimiter::MAX_ROTATIONAL_VELOCITY, + PandaRateLimiter::MAX_ROTATIONAL_ACCELERATION, + PandaRateLimiter::MAX_ROTATIONAL_JERK, &O_T_EE_c, &O_T_EE_c, &[0.; 6], @@ -907,4 +1160,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 = Fr3RateLimiter::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 = Fr3RateLimiter::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 = Fr3RateLimiter::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 = Fr3RateLimiter::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_data.rs b/src/robot/robot_data.rs index 55cf501..f7d2924 100644 --- a/src/robot/robot_data.rs +++ b/src/robot/robot_data.rs @@ -14,10 +14,11 @@ use crate::{FrankaResult, RobotModel, RobotState}; use serde::de::DeserializeOwned; use serde::Serialize; use std::fmt::Debug; +use crate::robot::rate_limiting::RateLimiterParameters; pub trait RobotData { - const RATE_LIMITING_ON_PER_DEFAULT: bool = false; type Model: RobotModel; + type RateLimiterParameters : RateLimiterParameters; type StateIntern: Debug + DeserializeOwned + Serialize + AbstractRobotStateIntern + 'static; type State: AbstractRobotState + From + From; } From 444b6b724dbcf7edebe9eb30af6e48a72b9385f6 Mon Sep 17 00:00:00 2001 From: Marco Boneberger Date: Tue, 17 Oct 2023 14:36:57 +0200 Subject: [PATCH 45/63] implement rate limiter directly in Fr3 and Panda struct --- src/robot.rs | 73 +++++++-- src/robot/control_loop.rs | 11 +- src/robot/control_types.rs | 12 +- src/robot/fr3.rs | 178 +++++++++++++++++++++- src/robot/panda.rs | 146 +++++++++++------- src/robot/private_robot.rs | 6 +- src/robot/rate_limiting.rs | 303 ++++--------------------------------- src/robot/robot_data.rs | 5 +- 8 files changed, 377 insertions(+), 357 deletions(-) diff --git a/src/robot.rs b/src/robot.rs index e1ce3d5..b501d8c 100644 --- a/src/robot.rs +++ b/src/robot.rs @@ -40,6 +40,58 @@ pub(crate) mod types; pub mod virtual_wall_cuboid; /// 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`](`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`](Robot::set_K). +/// +/// +/// # Motion generation and joint-level torque commands +/// +/// The following methods allow to perform motion generation and/or send joint-level torque +/// 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. +/// +/// 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 +/// be called again with the most recently received robot state. Since the robot is controlled with +/// 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 +/// 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::robot::control_types::{JointPositions, MotionFinished}; +/// use std::time::Duration; +/// # fn your_function_which_generates_joint_positions(time:f64) -> JointPositions {JointPositions::new([0.;7])} +/// let mut time = 0.; +/// let callback = |state: &RobotState, time_step: &Duration| -> JointPositions { +/// time += time_step.as_secs_f64(); +/// let out: JointPositions = your_function_which_generates_joint_positions(time); +/// if time >= 5.0 { +/// return out.motion_finished(); +/// } +/// return out; +/// }; +/// ``` +/// # Commands +/// +/// 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 trait Robot { type Model: RobotModel; @@ -78,7 +130,7 @@ pub trait Robot { /// * `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::RateLimiterParameters::RATE_LIMITING_ON_PER_DEFAULT`). + /// [`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. @@ -289,7 +341,7 @@ pub trait Robot { /// * `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::RateLimiterParameters::RATE_LIMITING_ON_PER_DEFAULT`). + /// [`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. @@ -325,7 +377,7 @@ pub trait Robot { /// * `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::RateLimiterParameters::RATE_LIMITING_ON_PER_DEFAULT`). + /// [`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. @@ -362,7 +414,7 @@ pub trait Robot { /// * `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::RateLimiterParameters::RATE_LIMITING_ON_PER_DEFAULT`). + /// [`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. @@ -396,8 +448,9 @@ pub trait Robot { /// # 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! + /// * `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`) @@ -435,7 +488,7 @@ pub trait Robot { /// 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::RateLimiterParameters::RATE_LIMITING_ON_PER_DEFAULT`). + /// [`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. @@ -472,7 +525,7 @@ pub trait Robot { /// 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::RateLimiterParameters::RATE_LIMITING_ON_PER_DEFAULT`). + /// [`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. @@ -509,7 +562,7 @@ pub trait Robot { /// 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::RateLimiterParameters::RATE_LIMITING_ON_PER_DEFAULT`). + /// [`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. @@ -546,7 +599,7 @@ pub trait Robot { /// 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::RateLimiterParameters::RATE_LIMITING_ON_PER_DEFAULT`). + /// [`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. diff --git a/src/robot/control_loop.rs b/src/robot/control_loop.rs index 934a092..3f786d8 100644 --- a/src/robot/control_loop.rs +++ b/src/robot/control_loop.rs @@ -10,7 +10,7 @@ use crate::robot::control_tools::{ 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, RateLimiterParameters}; +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; @@ -199,7 +199,7 @@ impl< } if self.limit_rate { control_output.tau_J = limit_rate_torques( - &Data::RateLimiterParameters::MAX_TORQUE_RATE, + &::MAX_TORQUE_RATE, &control_output.tau_J, &robot_state.get_tau_J_d(), ); @@ -215,7 +215,12 @@ impl< 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 1d05cf1..550fdc2 100644 --- a/src/robot/control_types.rs +++ b/src/robot/control_types.rs @@ -13,7 +13,7 @@ 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, RateLimiterParameters, DELTA_T, + limit_rate_joint_velocities, limit_rate_position, RateLimiter, DELTA_T, }; use crate::robot::robot_state::{AbstractRobotState, RobotState}; use crate::robot::service_types::MoveMotionGeneratorMode; @@ -37,7 +37,7 @@ pub enum RealtimeConfig { pub(crate) trait ConvertMotion { /// converts the motion type to a MotionGeneratorCommand and applies rate limiting and filtering - fn convert_motion( + fn convert_motion( &self, robot_state: &State, command: &mut MotionGeneratorCommand, @@ -140,7 +140,7 @@ impl Finishable for JointPositions { } } impl ConvertMotion for JointPositions { - fn convert_motion( + fn convert_motion( &self, robot_state: &RobotState, command: &mut MotionGeneratorCommand, @@ -215,7 +215,7 @@ impl Finishable for JointVelocities { } } impl ConvertMotion for JointVelocities { - fn convert_motion( + fn convert_motion( &self, robot_state: &RobotState, command: &mut MotionGeneratorCommand, @@ -331,7 +331,7 @@ impl Finishable for CartesianPose { } impl ConvertMotion for CartesianPose { - fn convert_motion( + fn convert_motion( &self, robot_state: &RobotState, command: &mut MotionGeneratorCommand, @@ -453,7 +453,7 @@ impl Finishable for CartesianVelocities { } } impl ConvertMotion for CartesianVelocities { - fn convert_motion( + fn convert_motion( &self, robot_state: &RobotState, command: &mut MotionGeneratorCommand, diff --git a/src/robot/fr3.rs b/src/robot/fr3.rs index 4b9e753..ca142cf 100644 --- a/src/robot/fr3.rs +++ b/src/robot/fr3.rs @@ -4,6 +4,7 @@ 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; @@ -18,13 +19,20 @@ use crate::robot::service_types::{ use crate::robot::types::Fr3StateIntern; use crate::{Fr3Model, FrankaResult, RealtimeConfig, RobotState}; use std::mem::size_of; -use crate::robot::rate_limiting::Fr3RateLimiter; +/// 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 = Fr3Model; - type RateLimiterParameters = Fr3RateLimiter; type StateIntern = Fr3StateIntern; type State = RobotState; } @@ -297,3 +305,169 @@ impl PrivateRobotData for Fr3 { } } } + +/// 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/panda.rs b/src/robot/panda.rs index ae88814..b288041 100644 --- a/src/robot/panda.rs +++ b/src/robot/panda.rs @@ -4,6 +4,7 @@ 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; @@ -22,61 +23,18 @@ use crate::robot::types::PandaStateIntern; use crate::robot::virtual_wall_cuboid::VirtualWallCuboid; use crate::{FrankaResult, PandaModel, RealtimeConfig, RobotState}; use std::mem::size_of; -use crate::robot::rate_limiting::PandaRateLimiter; /// Maintains a network connection to the robot, provides the current robot state, gives access to -/// the model library and allows to control the robot. -/// # 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`](`crate::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`](``crate::Robot::set_K`). +/// the model library and allows to control the robot. See the [`Robot`](crate::Robot) trait for +/// methods to control the robot. /// -/// -/// # Motion generation and joint-level torque commands -/// -/// The following methods allow to perform motion generation and/or send joint-level torque -/// 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. -/// -/// 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 -/// be called again with the most recently received robot state. Since the robot is controlled with -/// 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 -/// 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::robot::control_types::{JointPositions, MotionFinished}; -/// use std::time::Duration; -/// # fn your_function_which_generates_joint_positions(time:f64) -> JointPositions {JointPositions::new([0.;7])} -/// let mut time = 0.; -/// let callback = |state: &RobotState, time_step: &Duration| -> JointPositions { -/// time += time_step.as_secs_f64(); -/// let out: JointPositions = your_function_which_generates_joint_positions(time); -/// if time >= 5.0 { -/// return out.motion_finished(); -/// } -/// return out; -/// }; -/// ``` -/// # Commands -/// -/// 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. +/// # 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 { @@ -231,11 +189,93 @@ impl DeviceData for Panda { impl RobotData for Panda { type Model = PandaModel; - type RateLimiterParameters = PandaRateLimiter; 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; diff --git a/src/robot/private_robot.rs b/src/robot/private_robot.rs index 0ffac7a..6a0c0a4 100644 --- a/src/robot/private_robot.rs +++ b/src/robot/private_robot.rs @@ -1,6 +1,7 @@ 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::{ @@ -8,7 +9,6 @@ use crate::{ }; use std::fmt::Debug; use std::time::Duration; -use crate::robot::rate_limiting::RateLimiterParameters; pub(crate) trait PrivateRobot: PrivateRobotData + Sized { type Rob: RobotImplementation; @@ -27,7 +27,7 @@ pub(crate) trait PrivateRobot: PrivateRobotData + Sized { cutoff_frequency: Option, ) -> FrankaResult<()> { let controller_mode = controller_mode.unwrap_or(ControllerMode::JointImpedance); - let limit_rate = limit_rate.unwrap_or(::RateLimiterParameters::RATE_LIMITING_ON_PER_DEFAULT); + 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(), @@ -49,7 +49,7 @@ pub(crate) trait PrivateRobot: PrivateRobotData + Sized { limit_rate: Option, cutoff_frequency: Option, ) -> FrankaResult<()> { - let limit_rate = limit_rate.unwrap_or(::RateLimiterParameters::RATE_LIMITING_ON_PER_DEFAULT); + 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(), diff --git a/src/robot/rate_limiting.rs b/src/robot/rate_limiting.rs index 84b4680..7e7723e 100644 --- a/src/robot/rate_limiting.rs +++ b/src/robot/rate_limiting.rs @@ -21,7 +21,12 @@ pub const NORM_EPS: f64 = f64::EPSILON; /// Factor for the definition of rotational limits using the Cartesian Pose interface const FACTOR_CARTESIAN_ROTATION_POSE_INTERFACE: f64 = 0.99; -pub trait RateLimiterParameters { +/// 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 @@ -64,262 +69,6 @@ pub trait RateLimiterParameters { fn compute_lower_limits_joint_velocity(q: &[f64; 7]) -> [f64; 7]; } -/// A constant velocity rate limiter for the Panda robot. -/// This implementation is based on the rate limiter in the original libfranka library (version 0.9.2). -/// In this rate limiter the position is completely ignored for calculating the maximum and minimum -/// joint velocities. -pub struct PandaRateLimiter {} - -impl PandaRateLimiter { - /// 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 RateLimiterParameters for PandaRateLimiter { - 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 - } -} - -/// 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). -pub struct Fr3RateLimiter {} -impl Fr3RateLimiter { - /// 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 RateLimiterParameters for Fr3RateLimiter { - 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], - ), - ] - } -} - /// Limits the rate of a desired joint position considering the limits provided. /// # Note /// FCI filters must be deactivated to work properly. @@ -336,6 +85,7 @@ impl RateLimiterParameters for Fr3RateLimiter { /// * 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], @@ -380,6 +130,7 @@ 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, @@ -711,13 +462,11 @@ fn limit_rate_single_cartesian_velocity( #[cfg(test)] mod tests { + use crate::{Fr3, Panda}; use nalgebra::{Translation3, Unit, UnitQuaternion, Vector3, Vector6, U3}; use num_traits::Float; - use crate::robot::rate_limiting::{ - limit_rate_cartesian_pose, Fr3RateLimiter, PandaRateLimiter, RateLimiterParameters, - DELTA_T, LIMIT_EPS, - }; + 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] { @@ -890,12 +639,12 @@ mod tests { ]; let out = limit_rate_cartesian_pose( - PandaRateLimiter::MAX_TRANSLATIONAL_VELOCITY, - PandaRateLimiter::MAX_TRANSLATIONAL_ACCELERATION, - PandaRateLimiter::MAX_TRANSLATIONAL_JERK, - PandaRateLimiter::MAX_ROTATIONAL_VELOCITY, - PandaRateLimiter::MAX_ROTATIONAL_ACCELERATION, - PandaRateLimiter::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, @@ -927,12 +676,12 @@ mod tests { 1.0, ]; let out = limit_rate_cartesian_pose( - PandaRateLimiter::MAX_TRANSLATIONAL_VELOCITY, - PandaRateLimiter::MAX_TRANSLATIONAL_ACCELERATION, - PandaRateLimiter::MAX_TRANSLATIONAL_JERK, - PandaRateLimiter::MAX_ROTATIONAL_VELOCITY, - PandaRateLimiter::MAX_ROTATIONAL_ACCELERATION, - PandaRateLimiter::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], @@ -1167,13 +916,13 @@ mod tests { 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 = Fr3RateLimiter::compute_upper_limits_joint_velocity(&q_lower_limits); + 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 = Fr3RateLimiter::compute_lower_limits_joint_velocity(&q_lower_limits); + let dq_min = Fr3::compute_lower_limits_joint_velocity(&q_lower_limits); assert!(dq_max .iter() .zip(dq_lower_limits.iter()) @@ -1188,13 +937,13 @@ mod tests { 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 = Fr3RateLimiter::compute_upper_limits_joint_velocity(&q_upper_limits); + 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 = Fr3RateLimiter::compute_lower_limits_joint_velocity(&q_upper_limits); + let dq_min = Fr3::compute_lower_limits_joint_velocity(&q_upper_limits); assert!(dq_max .iter() .zip(dq_lower_limits.iter()) diff --git a/src/robot/robot_data.rs b/src/robot/robot_data.rs index f7d2924..fad6d90 100644 --- a/src/robot/robot_data.rs +++ b/src/robot/robot_data.rs @@ -3,6 +3,7 @@ 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, @@ -14,11 +15,9 @@ use crate::{FrankaResult, RobotModel, RobotState}; use serde::de::DeserializeOwned; use serde::Serialize; use std::fmt::Debug; -use crate::robot::rate_limiting::RateLimiterParameters; -pub trait RobotData { +pub trait RobotData: RateLimiter { type Model: RobotModel; - type RateLimiterParameters : RateLimiterParameters; type StateIntern: Debug + DeserializeOwned + Serialize + AbstractRobotStateIntern + 'static; type State: AbstractRobotState + From + From; } From b020bcbb775dfaf65adee061f54b44d555b8a7ee Mon Sep 17 00:00:00 2001 From: Marco Boneberger Date: Tue, 17 Oct 2023 14:54:33 +0200 Subject: [PATCH 46/63] improve docs and rearrange method order --- src/robot.rs | 782 +++++++++++++++++++++++++-------------------------- 1 file changed, 391 insertions(+), 391 deletions(-) diff --git a/src/robot.rs b/src/robot.rs index b501d8c..685a7aa 100644 --- a/src/robot.rs +++ b/src/robot.rs @@ -95,6 +95,16 @@ pub mod virtual_wall_cuboid; pub trait Robot { type Model: RobotModel; + /// Waits for a robot state update and returns it. + /// + /// # 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. @@ -119,20 +129,28 @@ pub trait Robot { /// * [`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. + /// * `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. + /// * `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 + /// * `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 @@ -156,194 +174,20 @@ pub trait Robot { cutoff_frequency: CF, ) -> 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<()>; - - /// Waits for a robot state update and returns it. - /// - /// # 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; - - /// Changes the collision behavior. - /// - /// Set separate torque and force boundaries for acceleration/deceleration and constant velocity - /// movement phases. - /// - /// Forces or torques between lower and upper threshold are shown as contacts in the RobotState. - /// Forces or torques above the upper threshold are registered as collision and cause the robot to - /// stop moving. - /// - /// # Arguments - /// * `lower_torque_thresholds_acceleration` - Contact torque thresholds during - /// acceleration/deceleration for each joint in \[Nm\] - /// * `upper_torque_thresholds_acceleration` - Collision torque thresholds during - /// acceleration/deceleration for each joint in \[Nm\] - /// * `lower_torque_thresholds_nominal` - Contact torque thresholds for each joint in \[Nm\] - /// * `upper_torque_thresholds_nominal` - Collision torque thresholds for each joint in \[Nm\] - /// * `lower_force_thresholds_acceleration` -Contact force thresholds during - /// acceleration/deceleration for (x,y,z,R,P,Y) in \[N\]. - /// * `upper_force_thresholds_acceleration` - Collision force thresholds during - /// acceleration/deceleration for (x,y,z,R,P,Y) in \[N\]. - /// * `lower_force_thresholds_nominal` - Contact force thresholds for (x,y,z,R,P,Y) in \[N\] - /// * `upper_force_thresholds_nominal` - Collision force thresholds for (x,y,z,R,P,Y) in \[N\] - /// # 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. - /// # See also - /// * [`RobotState::cartesian_contact`](`crate::RobotState::cartesian_contact`) - /// * [`RobotState::cartesian_collision`](`crate::RobotState::cartesian_collision`) - /// * [`RobotState::joint_contact`](`crate::RobotState::joint_contact`) - /// * [`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)] - fn set_collision_behavior( - &mut self, - 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<()>; - - /// 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. - /// - /// User-provided torques are not affected by this setting. - /// # Arguments - /// * `K_theta` - Joint impedance values ![K_{\theta}](https://latex.codecogs.com/png.latex?K_{\theta}). - /// # 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_joint_impedance(&mut self, K_theta: [f64; 7]) -> FrankaResult<()>; - - /// Sets the Cartesian impedance for (x, y, z, roll, pitch, yaw) in the internal controller. - /// - /// User-provided torques are not affected by this setting. - /// # Arguments - /// * `K_x` - Cartesian impedance values - /// - /// ![K_x=(x \in \[10,3000\] \frac{N}{m}, y \in \[10,3000\] \frac{N}{m}, z \in \[10,3000\] \frac{N}{m}, R \in \[1,300\] \frac{Nm}{rad}, P \in \[1,300\] \frac{Nm}{rad}, Y \in \[1,300\] \frac{Nm}{rad})](https://latex.codecogs.com/png.latex?K_x=(x&space;\in&space;[10,3000]&space;\frac{N}{m},&space;y&space;\in&space;[10,3000]&space;\frac{N}{m},&space;z&space;\in&space;[10,3000]&space;\frac{N}{m},&space;R&space;\in&space;[1,300]&space;\frac{Nm}{rad},&space;P&space;\in&space;[1,300]&space;\frac{Nm}{rad},&space;Y&space;\in&space;[1,300]&space;\frac{Nm}{rad})) - /// # 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_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. - /// # Note - /// Guiding mode can be enabled by pressing the two opposing buttons near the robot's flange. - /// # Arguments - /// * `guiding_mode` - Unlocked movement in (x, y, z, R, P, Y) in guiding mode. - /// * `elbow` - True if the elbow is free in guiding mode, false otherwise. - /// # 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_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. - /// - /// The transformation matrix is represented as a vectorized 4x4 matrix in column-major format. - /// # Arguments - /// * `EE_T_K` - Vectorized EE-to-K transformation matrix ![^{EE}T_K](https://latex.codecogs.com/png.latex?^{EE}T_K), 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. - /// - /// See [Stiffness frame K](#stiffness-frame-k) for an explanation of the stiffness frame. - #[allow(non_snake_case)] - 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. - /// - /// The transformation matrix is represented as a vectorized 4x4 matrix in column-major format. - /// # Arguments - /// * `NE_T_EE` - Vectorized NE-to-EE transformation matrix ![^{NE}T_{EE}](https://latex.codecogs.com/png.latex?^{NE}T_{EE}), 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. - /// - /// # See also - /// * [`RobotState::NE_T_EE`](`crate::RobotState::NE_T_EE`) - /// * [`RobotState::O_T_EE`](`crate::RobotState::O_T_EE`) - /// * [`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)] - fn set_EE(&mut self, NE_T_EE: [f64; 16]) -> FrankaResult<()>; - - ///Runs automatic error recovery on the robot. - /// - /// Automatic error recovery e.g. resets the robot after a collision occurred. - /// # 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 automatic_error_recovery(&mut self) -> FrankaResult<()>; - - /// Stops all currently running motions. - /// - /// If a control or motion generator loop is running in another thread, it will be preempted - /// with a [`ControlException`](`crate::exception::FrankaException::ControlException`). - /// # 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 stop(&mut self) -> 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. + /// * `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. + /// * `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 + /// * `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 @@ -372,14 +216,14 @@ pub trait Robot { /// Cannot be executed while another control or motion generator loop is active. /// /// # Arguments - /// * `motion_generator_callback` Callback function for motion generation. + /// * `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. + /// * `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 + /// * `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 @@ -409,14 +253,14 @@ pub trait Robot { /// Cannot be executed while another control or motion generator loop is active. /// /// # Arguments - /// * `motion_generator_callback` Callback function for motion generation. + /// * `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. + /// * `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 + /// * `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 @@ -476,21 +320,21 @@ pub trait Robot { cutoff_frequency: CF, ) -> FrankaResult<()>; - /// Starts a control loop for sending joint-level torque commands and joint velocities. + /// 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. + /// * `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. + /// * `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. + /// * `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 + /// * `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 @@ -499,9 +343,9 @@ pub trait Robot { /// * [`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, + /// * 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>, @@ -513,21 +357,21 @@ pub trait Robot { cutoff_frequency: CF, ) -> FrankaResult<()>; - /// Starts a control loop for sending joint-level torque commands and joint positions. + /// 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. + /// * `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. + /// * `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. + /// * `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 + /// * `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 @@ -536,9 +380,9 @@ pub trait Robot { /// * [`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, + /// * 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>, @@ -556,15 +400,15 @@ pub trait Robot { /// Cannot be executed while another control or motion generator loop is active. /// /// # Arguments - /// * `control_callback` Callback function providing joint-level torque commands. + /// * `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. + /// * `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. + /// * `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 + /// * `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 @@ -593,15 +437,15 @@ pub trait Robot { /// Cannot be executed while another control or motion generator loop is active. /// /// # Arguments - /// * `control_callback` Callback function providing joint-level torque commands. + /// * `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. + /// * `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. + /// * `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 + /// * `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 @@ -634,74 +478,37 @@ pub trait Robot { /// * [`NetworkException`](`crate::exception::FrankaException::NetworkException`) if the connection is lost, e.g. after a timeout. fn load_model(&mut self, persistent: bool) -> FrankaResult; - /// Returns the software version reported by the connected server. + /// Changes the collision behavior. /// - /// # 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>, -{ - type Model = ::Model; - - 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(()) - } - - fn control_joint_positions< - F: FnMut(&RobotState, &Duration) -> JointPositions, - CM: Into>, - L: Into>, - CF: Into>, - >( - &mut self, - 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) - }; - ::control_motion_intern( - self, - cb, - controller_mode.into(), - limit_rate.into(), - cutoff_frequency.into(), - ) - } - - 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 read_once(&mut self) -> FrankaResult { - match ::get_rob_mut(self).read_once() { - Ok(state) => Ok(state.into()), - Err(e) => Err(e), - } - } - + /// Set separate torque and force boundaries for acceleration/deceleration and constant velocity + /// movement phases. + /// + /// Forces or torques between lower and upper threshold are shown as contacts in the RobotState. + /// Forces or torques above the upper threshold are registered as collision and cause the robot to + /// stop moving. + /// + /// # Arguments + /// * `lower_torque_thresholds_acceleration` - Contact torque thresholds during + /// acceleration/deceleration for each joint in \[Nm\] + /// * `upper_torque_thresholds_acceleration` - Collision torque thresholds during + /// acceleration/deceleration for each joint in \[Nm\] + /// * `lower_torque_thresholds_nominal` - Contact torque thresholds for each joint in \[Nm\] + /// * `upper_torque_thresholds_nominal` - Collision torque thresholds for each joint in \[Nm\] + /// * `lower_force_thresholds_acceleration` -Contact force thresholds during + /// acceleration/deceleration for (x,y,z,R,P,Y) in \[N\]. + /// * `upper_force_thresholds_acceleration` - Collision force thresholds during + /// acceleration/deceleration for (x,y,z,R,P,Y) in \[N\]. + /// * `lower_force_thresholds_nominal` - Contact force thresholds for (x,y,z,R,P,Y) in \[N\] + /// * `upper_force_thresholds_nominal` - Collision force thresholds for (x,y,z,R,P,Y) in \[N\] + /// # 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. + /// # See also + /// * [`RobotState::cartesian_contact`](`crate::RobotState::cartesian_contact`) + /// * [`RobotState::cartesian_collision`](`crate::RobotState::cartesian_collision`) + /// * [`RobotState::joint_contact`](`crate::RobotState::joint_contact`) + /// * [`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)] fn set_collision_behavior( &mut self, @@ -713,134 +520,186 @@ where upper_force_thresholds_acceleration: [f64; 6], lower_force_thresholds_nominal: [f64; 6], upper_force_thresholds_nominal: [f64; 6], - ) -> FrankaResult<()> { - 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) - } + ) -> 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], - )?; - self.set_joint_impedance([3000., 3000., 3000., 2500., 2500., 2000., 2000.])?; - self.set_cartesian_impedance([3000., 3000., 3000., 300., 300., 300.])?; - Ok(()) - } + /// 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. + /// + /// User-provided torques are not affected by this setting. + /// # Arguments + /// * `K_theta` - Joint impedance values ![K_{\theta}](https://latex.codecogs.com/png.latex?K_{\theta}). + /// # 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_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) - } + 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. + /// + /// User-provided torques are not affected by this setting. + /// # Arguments + /// * `K_x` - Cartesian impedance values + /// + /// ![K_x=(x \in \[10,3000\] \frac{N}{m}, y \in \[10,3000\] \frac{N}{m}, z \in \[10,3000\] \frac{N}{m}, R \in \[1,300\] \frac{Nm}{rad}, P \in \[1,300\] \frac{Nm}{rad}, Y \in \[1,300\] \frac{Nm}{rad})](https://latex.codecogs.com/png.latex?K_x=(x&space;\in&space;[10,3000]&space;\frac{N}{m},&space;y&space;\in&space;[10,3000]&space;\frac{N}{m},&space;z&space;\in&space;[10,3000]&space;\frac{N}{m},&space;R&space;\in&space;[1,300]&space;\frac{Nm}{rad},&space;P&space;\in&space;[1,300]&space;\frac{Nm}{rad},&space;Y&space;\in&space;[1,300]&space;\frac{Nm}{rad})) + /// # 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_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) - } + 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<()> { - 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) - } + ) -> FrankaResult<()>; - 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) - } + /// Locks or unlocks guiding mode movement in (x, y, z, roll, pitch, yaw). + /// + /// If a flag is set to true, movement is unlocked. + /// # Note + /// Guiding mode can be enabled by pressing the two opposing buttons near the robot's flange. + /// # Arguments + /// * `guiding_mode` - Unlocked movement in (x, y, z, R, P, Y) in guiding mode. + /// * `elbow` - True if the elbow is free in guiding mode, false otherwise. + /// # 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_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. + /// + /// The transformation matrix is represented as a vectorized 4x4 matrix in column-major format. + /// # Arguments + /// * `EE_T_K` - Vectorized EE-to-K transformation matrix ![^{EE}T_K](https://latex.codecogs.com/png.latex?^{EE}T_K), 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. + /// + /// See [Stiffness frame K](#stiffness-frame-k) for an explanation of the stiffness frame. #[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) - } + 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. + /// + /// The transformation matrix is represented as a vectorized 4x4 matrix in column-major format. + /// # Arguments + /// * `NE_T_EE` - Vectorized NE-to-EE transformation matrix ![^{NE}T_{EE}](https://latex.codecogs.com/png.latex?^{NE}T_{EE}), 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. + /// + /// # See also + /// * [`RobotState::NE_T_EE`](`crate::RobotState::NE_T_EE`) + /// * [`RobotState::O_T_EE`](`crate::RobotState::O_T_EE`) + /// * [`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)] - 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 set_EE(&mut self, NE_T_EE: [f64; 16]) -> FrankaResult<()>; - 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) + ///Runs automatic error recovery on the robot. + /// + /// Automatic error recovery e.g. resets the robot after a collision occurred. + /// # 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 automatic_error_recovery(&mut self) -> FrankaResult<()>; + + /// Stops all currently running motions. + /// + /// If a control or motion generator loop is running in another thread, it will be preempted + /// with a [`ControlException`](`crate::exception::FrankaException::ControlException`). + /// # 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 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>, +{ + type Model = ::Model; + + fn read_once(&mut self) -> FrankaResult { + match ::get_rob_mut(self).read_once() { + Ok(state) => Ok(state.into()), + Err(e) => Err(e), + } } - 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", - )) + 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; } - 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!", - )), } + Ok(()) + } + + 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, + 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) + }; + ::control_motion_intern( + self, + cb, + controller_mode.into(), + limit_rate.into(), + cutoff_frequency.into(), + ) } fn control_joint_velocities< @@ -939,8 +798,8 @@ where ) } - fn control_torques_and_joint_velocities< - F: FnMut(&RobotState, &Duration) -> JointVelocities, + fn control_torques_and_joint_positions< + F: FnMut(&RobotState, &Duration) -> JointPositions, T: FnMut(&RobotState, &Duration) -> Torques, L: Into>, CF: Into>, @@ -965,8 +824,8 @@ where ) } - fn control_torques_and_joint_positions< - F: FnMut(&RobotState, &Duration) -> JointPositions, + fn control_torques_and_joint_velocities< + F: FnMut(&RobotState, &Duration) -> JointVelocities, T: FnMut(&RobotState, &Duration) -> Torques, L: Into>, CF: Into>, @@ -1047,6 +906,147 @@ where ::get_rob_mut(self).load_model(persistent) } + #[allow(clippy::too_many_arguments)] + fn set_collision_behavior( + &mut self, + 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 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) + } + + 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], + )?; + self.set_joint_impedance([3000., 3000., 3000., 2500., 2500., 2000., 2000.])?; + self.set_cartesian_impedance([3000., 3000., 3000., 300., 300., 300.])?; + Ok(()) + } + + #[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) + } + + #[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) + } + + #[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 server_version(&self) -> u16 { ::get_rob(self).server_version() } From 58c3256662b1aa44bc181811bf7cffe32b2dcb49 Mon Sep 17 00:00:00 2001 From: Marco Boneberger Date: Tue, 17 Oct 2023 15:18:55 +0200 Subject: [PATCH 47/63] update to nalgebra 0.32 --- Cargo.toml | 2 +- examples/cartesian_impedance_control.rs | 14 ++++++------ examples/mirror_robot.rs | 14 ++++++------ src/robot/rate_limiting.rs | 29 ++++++++++++------------- src/utils.rs | 8 +++---- 5 files changed, 33 insertions(+), 34 deletions(-) diff --git a/Cargo.toml b/Cargo.toml index de2c220..9e03619 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -79,7 +79,7 @@ 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" diff --git a/examples/cartesian_impedance_control.rs b/examples/cartesian_impedance_control.rs index 3c90ca5..5a069ec 100644 --- a/examples/cartesian_impedance_control.rs +++ b/examples/cartesian_impedance_control.rs @@ -4,7 +4,7 @@ use std::time::Duration; use clap::Parser; -use nalgebra::{Matrix3, Matrix6, Matrix6x1, UnitQuaternion, Vector3, U1, U3}; +use nalgebra::{Matrix3, Matrix6, Matrix6x1, UnitQuaternion, Vector3}; use franka::{ array_to_isometry, Fr3, Frame, FrankaResult, Matrix6x7, Panda, Robot, RobotModel, RobotState, @@ -49,15 +49,15 @@ fn generate_motion(mut robot: R) -> FrankaResult<()> { 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())); } @@ -93,7 +93,7 @@ fn generate_motion(mut robot: R) -> 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)); } @@ -103,7 +103,7 @@ fn generate_motion(mut robot: R) -> 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/mirror_robot.rs b/examples/mirror_robot.rs index bdfb2cb..145c593 100644 --- a/examples/mirror_robot.rs +++ b/examples/mirror_robot.rs @@ -6,7 +6,7 @@ use std::sync::mpsc::channel; use std::time::Duration; use clap::Parser; -use nalgebra::{Matrix3, Matrix6, Matrix6x1, Quaternion, UnitQuaternion, Vector3, U1, U3}; +use nalgebra::{Matrix3, Matrix6, Matrix6x1, Quaternion, UnitQuaternion, Vector3}; use franka::{ array_to_isometry, Fr3, Frame, FrankaResult, Matrix6x7, Matrix7, Panda, Robot, RobotModel, @@ -78,15 +78,15 @@ fn control_robots( 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())); } @@ -164,7 +164,7 @@ fn control_robots( 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)); } @@ -174,7 +174,7 @@ fn control_robots( 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/src/robot/rate_limiting.rs b/src/robot/rate_limiting.rs index 7e7723e..2f00e1c 100644 --- a/src/robot/rate_limiting.rs +++ b/src/robot/rate_limiting.rs @@ -5,8 +5,7 @@ //! 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; @@ -400,17 +399,17 @@ 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]; @@ -463,7 +462,7 @@ fn limit_rate_single_cartesian_velocity( #[cfg(test)] mod tests { use crate::{Fr3, Panda}; - use nalgebra::{Translation3, Unit, UnitQuaternion, Vector3, Vector6, U3}; + use nalgebra::{Translation3, Unit, UnitQuaternion, Vector3, Vector6}; use num_traits::Float; use crate::robot::rate_limiting::{limit_rate_cartesian_pose, RateLimiter, DELTA_T, LIMIT_EPS}; @@ -575,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)] diff --git a/src/utils.rs b/src/utils.rs index 08284b0..5ea667a 100644 --- a/src/utils.rs +++ b/src/utils.rs @@ -5,7 +5,7 @@ use crate::robot::control_types::JointPositions; use crate::robot::robot_state::AbstractRobotState; use crate::Finishable; -use nalgebra::{Isometry3, Matrix4, MatrixMN, MatrixN, Rotation3, Vector3, VectorN, U6, U7}; +use nalgebra::{Isometry3, Matrix4, Rotation3, SMatrix, SVector, Vector3}; use std::time::Duration; /// converts a 4x4 column-major homogenous matrix to an Isometry @@ -21,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). From 5b9cf0ef3d1e7ec557ffce2554ba532dcda276e9 Mon Sep 17 00:00:00 2001 From: Marco Boneberger Date: Tue, 17 Oct 2023 15:31:14 +0200 Subject: [PATCH 48/63] update to clap 4 --- Cargo.toml | 2 +- examples/download_model.rs | 1 - 2 files changed, 1 insertion(+), 2 deletions(-) diff --git a/Cargo.toml b/Cargo.toml index 9e03619..4702980 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -84,6 +84,6 @@ thiserror = "1.0" libloading = "0.7.0" [dev-dependencies] -clap = { version = "3.2.23", features = ["derive"] } +clap = { version = "4", features = ["derive"] } mockall = "0.9.1" float_extras = "0.1.6" diff --git a/examples/download_model.rs b/examples/download_model.rs index 58c0d5e..1acd872 100644 --- a/examples/download_model.rs +++ b/examples/download_model.rs @@ -17,7 +17,6 @@ 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)] From f0ddcb0910466419c8e636a00e7b2566efc5358b Mon Sep 17 00:00:00 2001 From: Marco Boneberger Date: Tue, 17 Oct 2023 16:13:43 +0200 Subject: [PATCH 49/63] set version to 0.10.0 and update README.md --- Cargo.toml | 2 +- README.md | 74 ++++++++++++++++++++++++++++++++++++++---------------- 2 files changed, 54 insertions(+), 22 deletions(-) diff --git a/Cargo.toml b/Cargo.toml index 4702980..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" diff --git a/README.md b/README.md index 47bc4d9..59b6b12 100644 --- a/README.md +++ b/README.md @@ -5,7 +5,7 @@ [![docs.rs](https://img.shields.io/docsrs/libfranka-rs?style=flat-square)](https://docs.rs/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 +15,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 @@ -30,19 +31,48 @@ Not supported: ## 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], @@ -53,11 +83,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(); @@ -70,14 +98,21 @@ 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 @@ -88,7 +123,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 ``` @@ -107,12 +142,9 @@ communication_test 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 From 03af643d5c67a65e3f41b062cfdcd0dc3308f57e Mon Sep 17 00:00:00 2001 From: Marco Boneberger Date: Wed, 18 Oct 2023 14:05:47 +0200 Subject: [PATCH 50/63] add code coverage --- .github/actions-rs/grcov.yml | 8 ++++++++ .github/workflows/rust.yml | 32 ++++++++++++++++++++++++++++++-- 2 files changed, 38 insertions(+), 2 deletions(-) create mode 100644 .github/actions-rs/grcov.yml 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..d682047 100644 --- a/.github/workflows/rust.yml +++ b/.github/workflows/rust.yml @@ -22,5 +22,33 @@ 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 + with: + verbose: true + fail_ci_if_error: true + files: lcov.info From 6155872f8023713dcad92f72c1c300ff711fd333 Mon Sep 17 00:00:00 2001 From: Marco Boneberger Date: Thu, 19 Oct 2023 11:03:32 +0200 Subject: [PATCH 51/63] add code coverage badge to README.md --- README.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/README.md b/README.md index 59b6b12..fa7706a 100644 --- a/README.md +++ b/README.md @@ -3,6 +3,8 @@ [![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 (Panda and FR3). From 7c1d5ce378e7f5fccc6e59c007855dd0b998a377 Mon Sep 17 00:00:00 2001 From: Marco Boneberger Date: Thu, 19 Oct 2023 14:10:13 +0200 Subject: [PATCH 52/63] Add CHANGELOG.md --- CHANGELOG.md | 53 ++++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 53 insertions(+) create mode 100644 CHANGELOG.md diff --git a/CHANGELOG.md b/CHANGELOG.md new file mode 100644 index 0000000..367d2a1 --- /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 (`RobotModel`) that is implemented by the `PandaModel` and `Fr3Model` structs +- 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 From 0a37e855afbb622db43fef346b3de89d753660b6 Mon Sep 17 00:00:00 2001 From: Marco Boneberger Date: Fri, 20 Oct 2023 11:23:26 +0200 Subject: [PATCH 53/63] fix echo_robot_state example --- examples/echo_robot_state.rs | 4 ---- 1 file changed, 4 deletions(-) diff --git a/examples/echo_robot_state.rs b/examples/echo_robot_state.rs index a23d53f..6a0eba8 100644 --- a/examples/echo_robot_state.rs +++ b/examples/echo_robot_state.rs @@ -31,10 +31,6 @@ fn main() -> FrankaResult<()> { } fn echo_robot_state(mut robot: R) -> FrankaResult<()> { - robot.set_collision_behavior( - [0.; 7], [0.; 7], [0.; 7], [0.; 7], [0.; 6], [0.; 6], [0.; 6], [0.; 6], - )?; - robot.set_joint_impedance([3000., 3000., 3000., 2500., 2500., 2000., 2000.])?; 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 From a8aff53b0843416dac436e2a657e2d39f5599253 Mon Sep 17 00:00:00 2001 From: Marco Boneberger Date: Fri, 20 Oct 2023 11:25:40 +0200 Subject: [PATCH 54/63] Add function to get model file path and use it in the download_model example --- examples/download_model.rs | 17 ++++++++++------- src/model.rs | 16 +++++++++++++++- 2 files changed, 25 insertions(+), 8 deletions(-) diff --git a/examples/download_model.rs b/examples/download_model.rs index 1acd872..608bfe6 100644 --- a/examples/download_model.rs +++ b/examples/download_model.rs @@ -7,7 +7,7 @@ use std::path::PathBuf; use clap::Parser; use franka::exception::FrankaException::ModelException; -use franka::{Fr3, FrankaResult, Panda, RealtimeConfig, Robot}; +use franka::{Fr3, FrankaResult, Panda, RealtimeConfig, Robot, RobotModel}; /// Downloads the model for offline usage #[derive(Parser, Debug)] @@ -25,8 +25,7 @@ struct CommandLineArguments { fn main() -> FrankaResult<()> { let args: CommandLineArguments = CommandLineArguments::parse(); - let mut path = args.download_path; - path.push("model.so"); + let path = args.download_path; match args.panda { true => { let robot = Panda::new(args.franka_ip.as_str(), RealtimeConfig::Ignore, None)?; @@ -39,10 +38,14 @@ fn main() -> FrankaResult<()> { } } -fn download_model(mut robot: R, path: PathBuf) -> FrankaResult<()> { - robot.load_model(true)?; - fs::copy("/tmp/model.so", &path).map_err(|_| ModelException { - message: "Could copy model to download location".to_string(), +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/src/model.rs b/src/model.rs index 3fe7c7f..7600f35 100644 --- a/src/model.rs +++ b/src/model.rs @@ -14,7 +14,7 @@ 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; @@ -114,6 +114,10 @@ pub trait RobotModel { /// * `EE_T_K` - Stiffness frame K in the end effector frame. /// # Return /// Vectorized 4x4 pose matrix, column-major. + + /// Returns the path where model is stored if it exists. + fn get_model_path(&self) -> Option; + #[allow(non_snake_case)] fn pose( &self, @@ -281,6 +285,7 @@ pub trait RobotModel { /// Calculates poses of joints and dynamic properties of a [Fr3](crate::Fr3). pub struct Fr3Model { library: ModelLibrary, + model_file: PathBuf, } #[allow(non_snake_case)] @@ -288,9 +293,18 @@ impl RobotModel for Fr3Model { fn new>(model_filename: S, libm_filename: Option<&Path>) -> FrankaResult { Ok(Fr3Model { 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, From 23b114c063ee6816bcd5aceca7b469ad575897ae Mon Sep 17 00:00:00 2001 From: Marco Boneberger Date: Fri, 20 Oct 2023 13:58:49 +0200 Subject: [PATCH 55/63] explicitly set codecov token in ci config --- .github/workflows/rust.yml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/.github/workflows/rust.yml b/.github/workflows/rust.yml index d682047..5fcbba3 100644 --- a/.github/workflows/rust.yml +++ b/.github/workflows/rust.yml @@ -48,6 +48,8 @@ jobs: ./target/debug/ - name: Codecov uses: codecov/codecov-action@v3 + env: + CODECOV_TOKEN: ${{ secrets.CODECOV_TOKEN }} with: verbose: true fail_ci_if_error: true From 52f83c6beca3ed0716b2dcb1795f84068993bf33 Mon Sep 17 00:00:00 2001 From: Marco Boneberger Date: Fri, 20 Oct 2023 14:32:31 +0200 Subject: [PATCH 56/63] Improved error handling for grasp_object example --- examples/grasp_object.rs | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/examples/grasp_object.rs b/examples/grasp_object.rs index 05b00cc..55de01f 100644 --- a/examples/grasp_object.rs +++ b/examples/grasp_object.rs @@ -30,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 { From 708b065cc34fcf682cc0fa6eefac742a2a561502 Mon Sep 17 00:00:00 2001 From: Marco Boneberger Date: Fri, 20 Oct 2023 15:04:02 +0200 Subject: [PATCH 57/63] minor improvements to documentation --- README.md | 7 ++++--- src/lib.rs | 4 ++-- src/model.rs | 8 ++++---- src/robot/fr3.rs | 23 +++++++++++++++++++++++ src/robot/panda.rs | 7 ++++--- 5 files changed, 37 insertions(+), 12 deletions(-) diff --git a/README.md b/README.md index fa7706a..4a7b959 100644 --- a/README.md +++ b/README.md @@ -117,7 +117,8 @@ fn main() -> FrankaResult<()> { ## 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 @@ -138,8 +139,8 @@ 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 diff --git a/src/lib.rs b/src/lib.rs index b575b0f..3cedde4 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -25,7 +25,7 @@ //! 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)?; +//! 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], @@ -129,7 +129,7 @@ //! # fn joint_positions() -> JointPositions { //! # let time = 0.; //! # let mut out = JointPositions::new([0.;7]); -//! +//! # //! if time >= 5.0 { //! return out.motion_finished(); //! } diff --git a/src/model.rs b/src/model.rs index 7600f35..886822b 100644 --- a/src/model.rs +++ b/src/model.rs @@ -104,6 +104,10 @@ pub trait RobotModel { 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. @@ -114,10 +118,6 @@ pub trait RobotModel { /// * `EE_T_K` - Stiffness frame K in the end effector frame. /// # Return /// Vectorized 4x4 pose matrix, column-major. - - /// Returns the path where model is stored if it exists. - fn get_model_path(&self) -> Option; - #[allow(non_snake_case)] fn pose( &self, diff --git a/src/robot/fr3.rs b/src/robot/fr3.rs index ca142cf..e14fe17 100644 --- a/src/robot/fr3.rs +++ b/src/robot/fr3.rs @@ -53,6 +53,29 @@ impl PrivateRobot for Fr3 { } 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, diff --git a/src/robot/panda.rs b/src/robot/panda.rs index b288041..bc79435 100644 --- a/src/robot/panda.rs +++ b/src/robot/panda.rs @@ -38,7 +38,7 @@ use std::mem::size_of; pub struct Panda(RobotImplGeneric); impl Panda { - /// Establishes a connection with the robot. + /// Establishes a connection with a Panda robot. /// /// # Arguments /// * `franka_address` - IP/hostname of the robot. @@ -58,8 +58,9 @@ impl Panda { /// } /// ``` /// # Errors - /// * [`NetworkException`](`crate::exception::FrankaException::NetworkException`) if the connection is unsuccessful. - /// * IncompatibleVersionException if this version of `libfranka-rs` is not supported. + /// * [`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, From 0c8b06b09fc7cd05323d7b3e2296bcfcf8672de3 Mon Sep 17 00:00:00 2001 From: Marco Boneberger Date: Mon, 30 Oct 2023 09:32:31 +0100 Subject: [PATCH 58/63] add copyright to files --- src/model/model_library.rs | 1 + src/network.rs | 1 + src/robot/control_loop.rs | 1 + src/robot/error.rs | 1 + src/robot/fr3.rs | 3 +++ src/robot/panda.rs | 3 +++ src/robot/private_robot.rs | 3 +++ src/robot/robot_control.rs | 1 + src/robot/robot_data.rs | 3 +++ src/robot/types.rs | 1 + 10 files changed, 18 insertions(+) diff --git a/src/model/model_library.rs b/src/model/model_library.rs index 1ec5d58..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; diff --git a/src/network.rs b/src/network.rs index 0fd27fb..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; diff --git a/src/robot/control_loop.rs b/src/robot/control_loop.rs index 3f786d8..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; 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/fr3.rs b/src/robot/fr3.rs index e14fe17..1090c47 100644 --- a/src/robot/fr3.rs +++ b/src/robot/fr3.rs @@ -1,3 +1,6 @@ +// 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; diff --git a/src/robot/panda.rs b/src/robot/panda.rs index bc79435..46c9d98 100644 --- a/src/robot/panda.rs +++ b/src/robot/panda.rs @@ -1,3 +1,6 @@ +// 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; diff --git a/src/robot/private_robot.rs b/src/robot/private_robot.rs index 6a0c0a4..8fed25f 100644 --- a/src/robot/private_robot.rs +++ b/src/robot/private_robot.rs @@ -1,3 +1,6 @@ +// 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; diff --git a/src/robot/robot_control.rs b/src/robot/robot_control.rs index 99a1f36..347e424 100644 --- a/src/robot/robot_control.rs +++ b/src/robot/robot_control.rs @@ -1,5 +1,6 @@ // 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::AbstractRobotState; diff --git a/src/robot/robot_data.rs b/src/robot/robot_data.rs index fad6d90..db2df54 100644 --- a/src/robot/robot_data.rs +++ b/src/robot/robot_data.rs @@ -1,3 +1,6 @@ +// 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; diff --git a/src/robot/types.rs b/src/robot/types.rs index 3e602ad..f1cf9b6 100644 --- a/src/robot/types.rs +++ b/src/robot/types.rs @@ -1,5 +1,6 @@ // Copyright (c) 2021 Marco Boneberger // Licensed under the EUPL-1.2-or-later + use std::fmt::Debug; use serde::Deserialize; From 6f73d2741d7b2ada595b78c15c8e83ab8ca4b8fe Mon Sep 17 00:00:00 2001 From: Marco Boneberger Date: Mon, 30 Oct 2023 10:25:04 +0100 Subject: [PATCH 59/63] add more documentation and improve some module exports --- src/lib.rs | 4 ++-- src/model.rs | 11 +++++++++++ src/model/library_downloader.rs | 2 +- src/robot.rs | 20 +++++++++++++------- src/robot/control_types.rs | 4 +++- src/robot/errors.rs | 2 +- src/robot/panda.rs | 2 +- src/robot/robot_data.rs | 4 ++++ src/robot/robot_state.rs | 2 +- src/robot/virtual_wall_cuboid.rs | 2 +- 10 files changed, 38 insertions(+), 15 deletions(-) diff --git a/src/lib.rs b/src/lib.rs index 3cedde4..46428b5 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -180,10 +180,10 @@ pub use model::Frame; pub use model::PandaModel; pub use model::RobotModel; pub use robot::control_types::*; -pub use robot::fr3::Fr3; pub use robot::low_pass_filter::DEFAULT_CUTOFF_FREQUENCY; pub use robot::low_pass_filter::MAX_CUTOFF_FREQUENCY; -pub use robot::panda::Panda; pub use robot::robot_state::RobotState; +pub use robot::Fr3; +pub use robot::Panda; pub use robot::Robot; pub use utils::*; diff --git a/src/model.rs b/src/model.rs index 886822b..206f07a 100644 --- a/src/model.rs +++ b/src/model.rs @@ -21,15 +21,26 @@ 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, } diff --git a/src/model/library_downloader.rs b/src/model/library_downloader.rs index e38ea91..76cb013 100644 --- a/src/model/library_downloader.rs +++ b/src/model/library_downloader.rs @@ -7,11 +7,11 @@ use std::path::Path; use crate::exception::FrankaException::ModelException; use crate::network::Network; -use crate::robot::robot_data::PrivateRobotData; use crate::robot::service_types::{ LoadModelLibraryArchitecture, LoadModelLibraryRequest, LoadModelLibraryStatus, LoadModelLibrarySystem, }; +use crate::robot::PrivateRobotData; use crate::FrankaResult; pub(crate) trait LibraryDownloader { diff --git a/src/robot.rs b/src/robot.rs index 685a7aa..7357b8a 100644 --- a/src/robot.rs +++ b/src/robot.rs @@ -1,12 +1,12 @@ // Copyright (c) 2021 Marco Boneberger // Licensed under the EUPL-1.2-or-later -//! Contains the franka::Robot type. +//! Contains the Robot trait and all modules that are necessary to use a robot. use crate::exception::create_command_exception; use crate::robot::private_robot::PrivateRobot; use crate::robot::robot_control::RobotControl; -use crate::robot::robot_data::{PrivateRobotData, RobotData}; +pub(crate) use crate::robot::robot_data::PrivateRobotData; use crate::robot::robot_impl::RobotImplementation; use crate::robot::service_types::{ SetCartesianImpedanceRequest, SetCollisionBehaviorRequest, SetEeToKRequest, @@ -15,7 +15,7 @@ use crate::robot::service_types::{ }; use crate::{ CartesianPose, CartesianVelocities, ControllerMode, FrankaResult, JointPositions, - JointVelocities, MotionGenerator, RobotModel, RobotState, Torques, MAX_CUTOFF_FREQUENCY, + JointVelocities, MotionGenerator, RobotModel, Torques, MAX_CUTOFF_FREQUENCY, }; use std::time::Duration; @@ -24,21 +24,26 @@ mod control_tools; pub mod control_types; pub mod error; pub mod errors; -pub mod fr3; +mod fr3; pub mod logger; pub mod low_pass_filter; mod motion_generator_traits; -pub mod panda; +mod panda; mod private_robot; pub mod rate_limiting; mod robot_control; -pub mod robot_data; +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; +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. @@ -93,6 +98,7 @@ pub mod virtual_wall_cuboid; /// These functions should therefore not be called from within control or motion generator loops. /// pub trait Robot { + /// Dynamic model of the robot. type Model: RobotModel; /// Waits for a robot state update and returns it. diff --git a/src/robot/control_types.rs b/src/robot/control_types.rs index 550fdc2..6ac5ca5 100644 --- a/src/robot/control_types.rs +++ b/src/robot/control_types.rs @@ -28,10 +28,12 @@ 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, } 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/panda.rs b/src/robot/panda.rs index 46c9d98..e5871a7 100644 --- a/src/robot/panda.rs +++ b/src/robot/panda.rs @@ -27,7 +27,7 @@ use crate::robot::virtual_wall_cuboid::VirtualWallCuboid; use crate::{FrankaResult, PandaModel, RealtimeConfig, RobotState}; use std::mem::size_of; -/// Maintains a network connection to the robot, provides the current robot state, gives access to +/// 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. /// diff --git a/src/robot/robot_data.rs b/src/robot/robot_data.rs index db2df54..a3e4467 100644 --- a/src/robot/robot_data.rs +++ b/src/robot/robot_data.rs @@ -19,9 +19,13 @@ 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: RobotModel; + /// 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; } diff --git a/src/robot/robot_state.rs b/src/robot/robot_state.rs index acd11f7..a33e45e 100644 --- a/src/robot/robot_state.rs +++ b/src/robot/robot_state.rs @@ -18,7 +18,7 @@ pub trait AbstractRobotState: Clone + Debug { fn get_q_d(&self) -> [f64; 7]; } -/// Describes the robot state. +/// Describes the state of the robot at a given time. #[derive(Debug, Clone, Default)] #[allow(non_snake_case)] pub struct RobotState { 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, From c93aba8b85648a9483b55aec18f9df3eb4e116e0 Mon Sep 17 00:00:00 2001 From: Marco Boneberger Date: Mon, 30 Oct 2023 10:55:07 +0100 Subject: [PATCH 60/63] Refactor `RobotModel` trait for unified modeling The Robot trait does not need a Model type anymore. Instead, it uses the RobotModel struct, which is an abstraction over the model for panda and fr3. Since currently fr3 and panda can work with the type, the RobotModel is also used internally. Should at a later point those two become incompatible, the internal model can be replaced with a type that converts into RobotModel --- CHANGELOG.md | 2 +- examples/cartesian_impedance_control.rs | 2 +- examples/download_model.rs | 2 +- examples/mirror_robot.rs | 2 +- examples/print_joint_poses.rs | 2 +- src/lib.rs | 3 +-- src/model.rs | 19 ++++++++----------- src/robot.rs | 13 +++++-------- src/robot/fr3.rs | 5 ++--- src/robot/panda.rs | 5 ++--- src/robot/robot_data.rs | 4 ++-- src/robot/robot_impl.rs | 2 +- 12 files changed, 26 insertions(+), 35 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 367d2a1..8e51d20 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -20,7 +20,7 @@ Requires Panda system version >= 4.2.1 ### Changed - The Robot struct became a trait that is implemented by the `Panda` and `Fr3` structs -- The Model struct became a trait (`RobotModel`) that is implemented by the `PandaModel` and `Fr3Model` 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 diff --git a/examples/cartesian_impedance_control.rs b/examples/cartesian_impedance_control.rs index 5a069ec..3857ffd 100644 --- a/examples/cartesian_impedance_control.rs +++ b/examples/cartesian_impedance_control.rs @@ -7,7 +7,7 @@ use clap::Parser; use nalgebra::{Matrix3, Matrix6, Matrix6x1, UnitQuaternion, Vector3}; use franka::{ - array_to_isometry, Fr3, Frame, FrankaResult, Matrix6x7, Panda, Robot, RobotModel, RobotState, + array_to_isometry, Fr3, Frame, FrankaResult, Matrix6x7, Model, Panda, Robot, RobotState, Torques, Vector7, }; diff --git a/examples/download_model.rs b/examples/download_model.rs index 608bfe6..05a1b3b 100644 --- a/examples/download_model.rs +++ b/examples/download_model.rs @@ -7,7 +7,7 @@ use std::path::PathBuf; use clap::Parser; use franka::exception::FrankaException::ModelException; -use franka::{Fr3, FrankaResult, Panda, RealtimeConfig, Robot, RobotModel}; +use franka::{Fr3, FrankaResult, Model, Panda, RealtimeConfig, Robot}; /// Downloads the model for offline usage #[derive(Parser, Debug)] diff --git a/examples/mirror_robot.rs b/examples/mirror_robot.rs index 145c593..241c1eb 100644 --- a/examples/mirror_robot.rs +++ b/examples/mirror_robot.rs @@ -9,7 +9,7 @@ use clap::Parser; use nalgebra::{Matrix3, Matrix6, Matrix6x1, Quaternion, UnitQuaternion, Vector3}; use franka::{ - array_to_isometry, Fr3, Frame, FrankaResult, Matrix6x7, Matrix7, Panda, Robot, RobotModel, + array_to_isometry, Fr3, Frame, FrankaResult, Matrix6x7, Matrix7, Model, Panda, Robot, RobotState, Torques, Vector7, }; diff --git a/examples/print_joint_poses.rs b/examples/print_joint_poses.rs index 493a29d..f707344 100644 --- a/examples/print_joint_poses.rs +++ b/examples/print_joint_poses.rs @@ -3,7 +3,7 @@ use clap::Parser; use nalgebra::Matrix4; -use franka::{Fr3, Frame, FrankaResult, Panda, Robot, RobotModel}; +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. diff --git a/src/lib.rs b/src/lib.rs index 46428b5..44b7799 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -175,9 +175,8 @@ pub mod utils; pub use exception::FrankaResult; pub use gripper::gripper_state::GripperState; pub use gripper::Gripper; -pub use model::Fr3Model; pub use model::Frame; -pub use model::PandaModel; +pub use model::Model; pub use model::RobotModel; pub use robot::control_types::*; pub use robot::low_pass_filter::DEFAULT_CUTOFF_FREQUENCY; diff --git a/src/model.rs b/src/model.rs index 206f07a..98e3518 100644 --- a/src/model.rs +++ b/src/model.rs @@ -3,10 +3,10 @@ //! Types for accessing the robots mass matrix, gravity torques and more. //! -//! [Fr3Model] is the Model for the FR3 and [PandaModel] is the Model for the Panda. -//! They are wrappers around a shared object that is received from the robot. +//! [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. -//! Both [Fr3Model] and [PandaModel] implement the [RobotModel] trait that +//! [RobotModel] implements the [Model] trait that //! contains the methods to get the needed info from the Model. use nalgebra::Matrix4; @@ -83,7 +83,7 @@ impl fmt::Display for Frame { /// Provides jacobians, mass matrix, coriolis torques, gravity torques and forward kinematics of /// a robot. -pub trait RobotModel { +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 @@ -293,16 +293,16 @@ pub trait RobotModel { ) -> [f64; 7]; } -/// Calculates poses of joints and dynamic properties of a [Fr3](crate::Fr3). -pub struct Fr3Model { +/// Calculates poses of joints and dynamic properties of a robot. +pub struct RobotModel { library: ModelLibrary, model_file: PathBuf, } #[allow(non_snake_case)] -impl RobotModel for Fr3Model { +impl Model for RobotModel { fn new>(model_filename: S, libm_filename: Option<&Path>) -> FrankaResult { - Ok(Fr3Model { + Ok(RobotModel { library: ModelLibrary::new(model_filename.as_ref(), libm_filename)?, model_file: model_filename.as_ref().to_path_buf(), }) @@ -511,6 +511,3 @@ impl RobotModel for Fr3Model { ) } } - -/// Calculates poses of joints and dynamic properties of a [Panda](crate::Panda). -pub type PandaModel = Fr3Model; diff --git a/src/robot.rs b/src/robot.rs index 7357b8a..1fee483 100644 --- a/src/robot.rs +++ b/src/robot.rs @@ -98,9 +98,6 @@ pub use robot_state::RobotState; /// These functions should therefore not be called from within control or motion generator loops. /// pub trait Robot { - /// Dynamic model of the robot. - type Model: RobotModel; - /// Waits for a robot state update and returns it. /// /// # Return @@ -482,7 +479,7 @@ pub trait Robot { /// # 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. - fn load_model(&mut self, persistent: bool) -> FrankaResult; + fn load_model(&mut self, persistent: bool) -> FrankaResult; /// Changes the collision behavior. /// @@ -655,8 +652,6 @@ where JointPositions: crate::ConvertMotion<::State>, CartesianVelocities: crate::ConvertMotion<::State>, { - type Model = ::Model; - fn read_once(&mut self) -> FrankaResult { match ::get_rob_mut(self).read_once() { Ok(state) => Ok(state.into()), @@ -908,8 +903,10 @@ where ) } - fn load_model(&mut self, persistent: bool) -> FrankaResult { - ::get_rob_mut(self).load_model(persistent) + fn load_model(&mut self, persistent: bool) -> FrankaResult { + ::get_rob_mut(self) + .load_model(persistent) + .map(|model| model.into()) } #[allow(clippy::too_many_arguments)] diff --git a/src/robot/fr3.rs b/src/robot/fr3.rs index 1090c47..cfd6b42 100644 --- a/src/robot/fr3.rs +++ b/src/robot/fr3.rs @@ -20,7 +20,7 @@ use crate::robot::service_types::{ SetLoadRequestWithFr3Header, SetNeToEeRequestWithFr3Header, StopMoveStatusFr3, FR3_VERSION, }; use crate::robot::types::Fr3StateIntern; -use crate::{Fr3Model, FrankaResult, RealtimeConfig, RobotState}; +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 @@ -35,7 +35,7 @@ use std::mem::size_of; pub struct Fr3(RobotImplGeneric); impl RobotData for Fr3 { - type Model = Fr3Model; + type Model = RobotModel; type StateIntern = Fr3StateIntern; type State = RobotState; } @@ -49,7 +49,6 @@ impl PrivateRobot for Fr3 { fn get_rob(&self) -> &Self::Rob { &self.0 } - fn get_net(&mut self) -> &mut Network { &mut self.0.network } diff --git a/src/robot/panda.rs b/src/robot/panda.rs index e5871a7..fbcd2e5 100644 --- a/src/robot/panda.rs +++ b/src/robot/panda.rs @@ -24,7 +24,7 @@ use crate::robot::service_types::{ }; use crate::robot::types::PandaStateIntern; use crate::robot::virtual_wall_cuboid::VirtualWallCuboid; -use crate::{FrankaResult, PandaModel, RealtimeConfig, RobotState}; +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 @@ -167,7 +167,6 @@ impl PrivateRobot for Panda { fn get_rob(&self) -> &Self::Rob { &self.0 } - fn get_net(&mut self) -> &mut Network { &mut self.0.network } @@ -192,7 +191,7 @@ impl DeviceData for Panda { } impl RobotData for Panda { - type Model = PandaModel; + type Model = RobotModel; type StateIntern = PandaStateIntern; type State = RobotState; } diff --git a/src/robot/robot_data.rs b/src/robot/robot_data.rs index a3e4467..0cb6abf 100644 --- a/src/robot/robot_data.rs +++ b/src/robot/robot_data.rs @@ -14,7 +14,7 @@ use crate::robot::service_types::{ SetGuidingModeRequest, SetJointImpedanceRequest, SetLoadRequest, SetNeToEeRequest, }; use crate::robot::types::AbstractRobotStateIntern; -use crate::{FrankaResult, RobotModel, RobotState}; +use crate::{FrankaResult, Model, RobotModel, RobotState}; use serde::de::DeserializeOwned; use serde::Serialize; use std::fmt::Debug; @@ -22,7 +22,7 @@ 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: RobotModel; + 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. diff --git a/src/robot/robot_impl.rs b/src/robot/robot_impl.rs index ad1f4a5..b427c36 100644 --- a/src/robot/robot_impl.rs +++ b/src/robot/robot_impl.rs @@ -19,7 +19,7 @@ use crate::robot::types::{ AbstractRobotStateIntern, ControllerCommand, ControllerMode, MotionGeneratorCommand, MotionGeneratorMode, RobotCommand, }; -use crate::RobotModel; +use crate::Model; use std::fs::remove_file; use std::path::Path; From e3f97f9442e258e3c003cb8d81e653e3048583fb Mon Sep 17 00:00:00 2001 From: Marco Boneberger Date: Mon, 30 Oct 2023 11:08:37 +0100 Subject: [PATCH 61/63] fix test --- src/robot.rs | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/robot.rs b/src/robot.rs index 1fee483..c932293 100644 --- a/src/robot.rs +++ b/src/robot.rs @@ -71,14 +71,14 @@ pub use robot_state::RobotState; /// 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])} From 55eabc99d23d3bf9fe3a3612a12002bf79fc9b4c Mon Sep 17 00:00:00 2001 From: Marco Boneberger Date: Mon, 30 Oct 2023 11:24:34 +0100 Subject: [PATCH 62/63] improve gripper imports --- src/gripper.rs | 34 ++++++++++++++++++---------------- src/lib.rs | 4 ++-- 2 files changed, 20 insertions(+), 18 deletions(-) diff --git a/src/gripper.rs b/src/gripper.rs index e4d505b..e69830a 100644 --- a/src/gripper.rs +++ b/src/gripper.rs @@ -3,11 +3,12 @@ //! Contains the franka::Gripper type. -use crate::device_data::DeviceData; 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::{ ConnectRequest, ConnectRequestWithHeader, ConnectResponse, GraspRequest, GraspRequestWithHeader, GripperCommandEnum, GripperCommandHeader, GripperStateIntern, @@ -15,7 +16,7 @@ use crate::gripper::types::{ }; 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, @@ -196,29 +197,30 @@ fn handle_response_status(status: &Status) -> FrankaResult { #[cfg(test)] mod tests { - use crate::gripper::types::{ - ConnectRequestWithHeader, ConnectResponse, GraspRequest, GraspRequestWithHeader, - GripperCommandEnum, GripperCommandHeader, GripperStateIntern, MoveRequest, - MoveRequestWithHeader, Status, COMMAND_PORT, GRIPPER_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 serde::{Deserialize, Serialize}; - use std::iter::FromIterator; - use std::mem::size_of; + use crate::FrankaResult; #[derive(Serialize, Deserialize, Clone, Copy)] #[repr(packed)] diff --git a/src/lib.rs b/src/lib.rs index 44b7799..b4444ea 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -173,7 +173,7 @@ pub mod model; pub mod utils; pub use exception::FrankaResult; -pub use gripper::gripper_state::GripperState; +pub use gripper::GripperState; pub use gripper::Gripper; pub use model::Frame; pub use model::Model; @@ -181,7 +181,7 @@ 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::RobotState; pub use robot::Fr3; pub use robot::Panda; pub use robot::Robot; From 367c69dcfc11434764a0dd6b70a3960d59e09bbc Mon Sep 17 00:00:00 2001 From: Marco Boneberger Date: Mon, 30 Oct 2023 11:32:05 +0100 Subject: [PATCH 63/63] fix formatting in lib.rs --- src/lib.rs | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/lib.rs b/src/lib.rs index b4444ea..6d913b2 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -173,16 +173,16 @@ pub mod model; pub mod utils; pub use exception::FrankaResult; -pub use gripper::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::RobotState; pub use robot::Fr3; pub use robot::Panda; pub use robot::Robot; +pub use robot::RobotState; pub use utils::*;