// Copyright (c) 2021 Marco Boneberger
// Licensed under the EUPL-1.2-or-later
//! Contains the franka::Robot type.
use std::mem::size_of;
use std::time::Duration;
use std::fmt::Debug;
use crate::exception::{create_command_exception, FrankaException, FrankaResult};
use crate::model::Model;
use crate::network::{Network, NetworkType};
use crate::robot::control_loop::ControlLoop;
use crate::robot::control_types::{
CartesianPose, CartesianVelocities, ControllerMode, Finishable, JointPositions,
JointVelocities, RealtimeConfig, Torques,
};
use crate::robot::low_pass_filter::{kDefaultCutoffFrequency, kMaxCutoffFrequency};
use crate::robot::motion_generator_traits::MotionGeneratorTrait;
use crate::robot::robot_control::RobotControl;
use crate::robot::robot_impl::RobotImpl;
use crate::robot::service_types::{
AutomaticErrorRecoveryRequestWithHeader, AutomaticErrorRecoveryResponse,
AutomaticErrorRecoveryStatus, GetCartesianLimitRequest, GetCartesianLimitRequestWithHeader,
GetCartesianLimitResponse, GetterSetterStatus, RobotCommandEnum, SetCartesianImpedanceRequest,
SetCartesianImpedanceRequestWithHeader, SetCartesianImpedanceResponse,
SetCollisionBehaviorRequest, SetCollisionBehaviorRequestWithHeader,
SetCollisionBehaviorResponse, SetEeToKRequest, SetEeToKRequestWithHeader, SetEeToKResponse,
SetFiltersRequest, SetFiltersRequestWithHeader, SetFiltersResponse, SetGuidingModeRequest,
SetGuidingModeRequestWithHeader, SetGuidingModeResponse, SetJointImpedanceRequest,
SetJointImpedanceRequestWithHeader, SetJointImpedanceResponse, SetLoadRequest,
SetLoadRequestWithHeader, SetLoadResponse, SetNeToEeRequest, SetNeToEeRequestWithHeader,
SetNeToEeResponse, StopMoveRequestWithHeader, StopMoveResponse, StopMoveStatus,
};
use crate::robot::virtual_wall_cuboid::VirtualWallCuboid;
use crate::utils::MotionGenerator;
use robot_state::RobotState;
mod control_loop;
mod control_tools;
pub mod control_types;
pub mod error;
pub mod errors;
pub mod logger;
pub mod low_pass_filter;
mod motion_generator_traits;
mod rate_limiting;
mod robot_control;
mod robot_impl;
pub mod robot_state;
pub(crate) mod service_types;
pub(crate) mod types;
pub mod virtual_wall_cuboid;
/// Maintains a network connection to the robot, provides the current robot state, gives access to
/// the model library and allows to control the robot.
/// # 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 Robot {
robimpl: RobotImpl,
}
impl Robot {
/// 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, Robot};
/// fn main() -> FrankaResult<()> {
/// // connects to the robot using real-time scheduling and a default log size of 50.
/// let mut robot = Robot::new("robotik-bs.de", None, None)?;
/// // connects to the robot without using real-time scheduling and a log size of 1.
/// let mut robot = Robot::new("robotik-bs.de", RealtimeConfig::kIgnore, 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<RtConfig: Into<Option<RealtimeConfig>>, LogSize: Into<Option<usize>>>(
franka_address: &str,
realtime_config: RtConfig,
log_size: LogSize,
) -> FrankaResult<Robot> {
let realtime_config = realtime_config.into().unwrap_or(RealtimeConfig::kEnforce);
let log_size = log_size.into().unwrap_or(50);
let network = Network::new(
NetworkType::Robot,
franka_address,
service_types::kCommandPort,
)
.map_err(|_| FrankaException::NetworkException {
message: "Connection could not be established".to_string(),
})?;
Ok(Robot {
robimpl: RobotImpl::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::{Robot, RobotState, FrankaResult};
/// fn main() -> FrankaResult<()> {
/// let mut robot = Robot::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 aslong
/// 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<F: FnMut(&RobotState) -> 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<RobotState> {
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::kSetCollisionBehavior,
size_of::<SetCollisionBehaviorRequestWithHeader>(),
),
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 .
/// # 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::kSetJointImpedance,
size_of::<SetJointImpedanceRequestWithHeader>(),
),
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::kSetCartesianImpedance,
size_of::<SetCartesianImpedanceRequestWithHeader>(),
),
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_robot(
RobotCommandEnum::kSetGuidingMode,
size_of::<SetGuidingModeRequestWithHeader>(),
),
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);
// println!("{:?}", response);
handle_getter_setter_status(response.status)
}
/// Sets the transformation  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 , 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_robot(
RobotCommandEnum::kSetEeToK,
size_of::<SetEeToKRequestWithHeader>(),
),
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  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 , 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_robot(
RobotCommandEnum::kSetNeToEe,
size_of::<SetNeToEeRequestWithHeader>(),
),
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
///  in \[m\]
/// * `load_inertia` - Inertia matrix  in [kg \times m^2], column-major.
/// # Errors
/// * [`CommandException`](`crate::exception::FrankaException::CommandException`) if the Control reports an error.
/// * [`NetworkException`](`crate::exception::FrankaException::NetworkException`) if the connection is lost, e.g. after a timeout.
#[allow(non_snake_case)]
pub fn set_load(
&mut self,
load_mass: f64,
F_x_Cload: [f64; 3],
load_inertia: [f64; 9],
) -> FrankaResult<()> {
let command = SetLoadRequestWithHeader {
header: self.robimpl.network.create_header_for_robot(
RobotCommandEnum::kSetLoad,
size_of::<SetLoadRequestWithHeader>(),
),
request: SetLoadRequest::new(load_mass, F_x_Cload, load_inertia),
};
let command_id: u32 = self.robimpl.network.tcp_send_request(command);
let response: SetLoadResponse = self
.robimpl
.network
.tcp_blocking_receive_response(command_id);
handle_getter_setter_status(response.status)
}
/// Sets the cut off frequency for the given motion generator or controller.
///
/// Allowed input range for all the filters is between 1.0 Hz and 1000.0 Hz.
/// If the value is set to maximum (1000Hz) then no filtering is done.
/// # Arguments
/// * `joint_position_filter_frequency` - Frequency at which the commanded joint
/// position is cut off.
/// * `joint_velocity_filter_frequency` - TFrequency at which the commanded joint
/// velocity is cut off.
/// * `cartesian_position_filter_frequency` - Frequency at which the commanded
/// Cartesian position is cut off.
/// * `cartesian_velocity_filter_frequency` - Frequency at which the commanded
/// Cartesian velocity is cut off.
/// * `controller_filter_frequency` - Frequency at which the commanded torque is cut off
/// # Errors
/// * [`CommandException`](`crate::exception::FrankaException::CommandException`) if the Control reports an error.
/// * [`NetworkException`](`crate::exception::FrankaException::NetworkException`) if the connection is lost, e.g. after a timeout.
#[deprecated(note = "please use `low_pass_filter` instead")]
pub fn set_filters(
&mut self,
joint_position_filter_frequency: f64,
joint_velocity_filter_frequency: f64,
cartesian_position_filter_frequency: f64,
cartesian_velocity_filter_frequency: f64,
controller_filter_frequency: f64,
) -> FrankaResult<()> {
let command = SetFiltersRequestWithHeader {
header: self.robimpl.network.create_header_for_robot(
RobotCommandEnum::kSetFilters,
size_of::<SetFiltersRequestWithHeader>(),
),
request: SetFiltersRequest::new(
joint_position_filter_frequency,
joint_velocity_filter_frequency,
cartesian_position_filter_frequency,
cartesian_velocity_filter_frequency,
controller_filter_frequency,
),
};
let command_id: u32 = self.robimpl.network.tcp_send_request(command);
let response: SetFiltersResponse = self
.robimpl
.network
.tcp_blocking_receive_response(command_id);
handle_getter_setter_status(response.status)
}
///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_robot(
RobotCommandEnum::kAutomaticErrorRecovery,
size_of::<AutomaticErrorRecoveryRequestWithHeader>(),
);
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::kSuccess => Ok(()),
AutomaticErrorRecoveryStatus::kEmergencyAborted => Err(create_command_exception(
"libfranka-rs: command aborted: User Stop pressed!",
)),
AutomaticErrorRecoveryStatus::kReflexAborted => Err(create_command_exception(
"libfranka-rs: command aborted: motion aborted by reflex!",
)),
AutomaticErrorRecoveryStatus::kCommandNotPossibleRejected => {
Err(create_command_exception(
"libfranka-rs: command rejected: command not possible in current mode",
))
}
AutomaticErrorRecoveryStatus::kManualErrorRecoveryRequiredRejected => {
Err(create_command_exception(
"libfranka-rs: command rejected: manual error recovery required!",
))
}
AutomaticErrorRecoveryStatus::kAborted => {
Err(create_command_exception("libfranka-rs: command aborted!"))
}
}
}
/// 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_robot(
RobotCommandEnum::kStopMove,
size_of::<StopMoveRequestWithHeader>(),
),
};
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::kSuccess => Ok(()),
StopMoveStatus::kCommandNotPossibleRejected | StopMoveStatus::kAborted => {
Err(create_command_exception(
"libfranka-rs: command rejected: command not possible in current mode",
))
}
StopMoveStatus::kEmergencyAborted => Err(create_command_exception(
"libfranka-rs: command aborted: User Stop pressed!",
)),
StopMoveStatus::kReflexAborted => 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.
/// # 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<VirtualWallCuboid> {
let command = GetCartesianLimitRequestWithHeader {
header: self.robimpl.network.create_header_for_robot(
RobotCommandEnum::kGetCartesianLimit,
size_of::<GetCartesianLimitRequestWithHeader>(),
),
request: GetCartesianLimitRequest::new(id),
};
let command_id: u32 = self.robimpl.network.tcp_send_request(command);
let response: GetCartesianLimitResponse = self
.robimpl
.network
.tcp_blocking_receive_response(command_id);
match &response.status {
GetterSetterStatus::kSuccess => Ok(VirtualWallCuboid::new(id, response)),
GetterSetterStatus::kCommandNotPossibleRejected => Err(create_command_exception(
"libfranka-rs: command rejected: command not possible in current mode",
)),
GetterSetterStatus::kInvalidArgumentRejected => 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 [`kMaxCutoffFrequency`](`crate::robot::low_pass_filter::kMaxCutoffFrequency`) 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(&RobotState, &Duration) -> JointPositions,
CM: Into<Option<ControllerMode>>,
L: Into<Option<bool>>,
CF: Into<Option<f64>>,
>(
&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 [`kMaxCutoffFrequency`](`crate::robot::low_pass_filter::kMaxCutoffFrequency`) 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(&RobotState, &Duration) -> JointVelocities,
CM: Into<Option<ControllerMode>>,
L: Into<Option<bool>>,
CF: Into<Option<f64>>,
>(
&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 [`kMaxCutoffFrequency`](`crate::robot::low_pass_filter::kMaxCutoffFrequency`) 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(&RobotState, &Duration) -> CartesianPose,
CM: Into<Option<ControllerMode>>,
L: Into<Option<bool>>,
CF: Into<Option<f64>>,
>(
&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 [`kMaxCutoffFrequency`](`crate::robot::low_pass_filter::kMaxCutoffFrequency`) 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(&RobotState, &Duration) -> CartesianVelocities,
CM: Into<Option<ControllerMode>>,
L: Into<Option<bool>>,
CF: Into<Option<f64>>,
>(
&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 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 [`kMaxCutoffFrequency`](`crate::robot::low_pass_filter::kMaxCutoffFrequency`) to disable.
/// Default is 100 Hz
/// # Errors
/// * [`ControlException`](`crate::exception::FrankaException::ControlException`) if an error related to torque control or motion generation occurred.
/// * [`NetworkException`](`crate::exception::FrankaException::NetworkException`) if the connection is lost, e.g. after a timeout.
/// * [`RealTimeException`](`crate::exception::FrankaException::RealTimeException`) if realtime priority cannot be set for the current thread.
/// # Panics
/// * if joint-level torque or joint velocity commands are NaN or infinity.
///
/// See [`new`](`Self::new`) to change behavior if realtime priority cannot be set.
pub fn control_torques_and_joint_velocities<
F: FnMut(&RobotState, &Duration) -> JointVelocities,
T: FnMut(&RobotState, &Duration) -> Torques,
L: Into<Option<bool>>,
CF: Into<Option<f64>>,
>(
&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 [`kMaxCutoffFrequency`](`crate::robot::low_pass_filter::kMaxCutoffFrequency`) 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(&RobotState, &Duration) -> JointPositions,
T: FnMut(&RobotState, &Duration) -> Torques,
L: Into<Option<bool>>,
CF: Into<Option<f64>>,
>(
&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 [`kMaxCutoffFrequency`](`crate::robot::low_pass_filter::kMaxCutoffFrequency`) to disable.
/// Default is 100 Hz
/// # Errors
/// * [`ControlException`](`crate::exception::FrankaException::ControlException`) if an error related to torque control or motion generation occurred.
/// * [`NetworkException`](`crate::exception::FrankaException::NetworkException`) if the connection is lost, e.g. after a timeout.
/// * [`RealTimeException`](`crate::exception::FrankaException::RealTimeException`) if realtime priority cannot be set for the current thread.
/// # Panics
/// * if joint-level torque or Cartesian pose command elements are NaN or infinity.
///
/// See [`new`](`Self::new`) to change behavior if realtime priority cannot be set.
pub fn control_torques_and_cartesian_pose<
F: FnMut(&RobotState, &Duration) -> CartesianPose,
T: FnMut(&RobotState, &Duration) -> Torques,
L: Into<Option<bool>>,
CF: Into<Option<f64>>,
>(
&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 [`kMaxCutoffFrequency`](`crate::robot::low_pass_filter::kMaxCutoffFrequency`) to disable.
/// Default is 100 Hz
/// # Errors
/// * [`ControlException`](`crate::exception::FrankaException::ControlException`) if an error related to torque control or motion generation occurred.
/// * [`NetworkException`](`crate::exception::FrankaException::NetworkException`) if the connection is lost, e.g. after a timeout.
/// * [`RealTimeException`](`crate::exception::FrankaException::RealTimeException`) if realtime priority cannot be set for the current thread.
/// # Panics
/// * if joint-level torque or Cartesian velocity command elements are NaN or infinity.
///
/// See [`new`](`Self::new`) to change behavior if realtime priority cannot be set.
pub fn control_torques_and_cartesian_velocities<
F: FnMut(&RobotState, &Duration) -> CartesianVelocities,
T: FnMut(&RobotState, &Duration) -> Torques,
L: Into<Option<bool>>,
CF: Into<Option<f64>>,
>(
&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
/// [`kMaxCutoffFrequency`](`crate::robot::low_pass_filter::kMaxCutoffFrequency`)
/// 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(&RobotState, &Duration) -> Torques,
L: Into<Option<bool>>,
CF: Into<Option<f64>>,
>(
&mut self,
mut control_callback: T,
limit_rate: L,
cutoff_frequency: CF,
) -> FrankaResult<()> {
let motion_generator_callback =
|_state: &RobotState, _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(&RobotState, &Duration) -> U,
U: Finishable + Debug + MotionGeneratorTrait,
>(
&mut self,
motion_generator_callback: F,
control_callback: &mut dyn FnMut(&RobotState, &Duration) -> Torques,
limit_rate: Option<bool>,
cutoff_frequency: Option<f64>,
) -> FrankaResult<()> {
let limit_rate = limit_rate.unwrap_or(true);
let cutoff_frequency = cutoff_frequency.unwrap_or(kDefaultCutoffFrequency);
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(&RobotState, &Duration) -> U,
U: Finishable + Debug + MotionGeneratorTrait,
>(
&mut self,
motion_generator_callback: F,
controller_mode: Option<ControllerMode>,
limit_rate: Option<bool>,
cutoff_frequency: Option<f64>,
) -> FrankaResult<()> {
let controller_mode = controller_mode.unwrap_or(ControllerMode::kJointImpedance);
let limit_rate = limit_rate.unwrap_or(true);
let cutoff_frequency = cutoff_frequency.unwrap_or(kDefaultCutoffFrequency);
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.
/// * [`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::kJointImpedance),
Some(true),
Some(kMaxCutoffFrequency),
)
}
/// 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<Model> {
self.robimpl.load_model(persistent)
}
/// 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::kSuccess => Ok(()),
GetterSetterStatus::kCommandNotPossibleRejected => Err(create_command_exception(
"libfranka-rs: command rejected: command not possible in current mode",
)),
GetterSetterStatus::kInvalidArgumentRejected => Err(create_command_exception(
"libfranka-rs: command rejected: invalid argument!",
)),
}
}
#[cfg(test)]
mod tests {
use mockall::{automock, predicate::*};
use std::io::{Read, Write};
use std::net::TcpListener;
use std::net::ToSocketAddrs;
use std::rc::Rc;
use std::sync::{Arc, Mutex};
use crate::exception::FrankaException;
use crate::network::MessageCommand;
use crate::robot::robot_impl::kVersion;
use crate::robot::service_types::{
kCommandPort, ConnectRequestWithHeader, ConnectResponse, ConnectStatus, GetterSetterStatus,
MoveControllerMode, MoveDeviation, MoveMotionGeneratorMode, MoveRequest,
MoveRequestWithHeader, MoveResponse, MoveStatus, RobotCommandEnum, RobotCommandHeader,
SetCollisionBehaviorRequest, SetCollisionBehaviorRequestWithHeader,
SetCollisionBehaviorResponse,
};
use crate::robot::types::RobotStateIntern;
use crate::{FrankaResult, JointPositions, MotionFinished, RealtimeConfig, Robot, RobotState};
use bincode::{deserialize, serialize, serialized_size};
use std::iter::FromIterator;
use std::mem::size_of;
use std::time::{Duration, Instant};
struct Socket<F: Fn(&Vec<u8>), G: Fn(&mut Vec<u8>)> {
pub send_bytes: F,
pub receive_bytes: G,
}
struct RobotMockServer {
server_version: u16,
}
pub struct ServerReaction {}
#[automock]
#[allow(unused)]
impl ServerReaction {
fn process_received_bytes(&self, bytes: &mut Vec<u8>) -> Vec<u8> {
Vec::new()
}
fn number_of_reactions(&self) -> usize {
0
}
}
impl RobotMockServer {
pub fn new(server_version: u16) -> Self {
RobotMockServer { server_version }
}
pub fn server_thread(&mut self, reaction: &mut MockServerReaction) {
let hostname: &str = "127.0.0.1";
let address = format!("{}:{}", hostname, kCommandPort)
.to_socket_addrs()
.unwrap()
.next()
.unwrap();
let srv_sock = TcpListener::bind(address).unwrap();
let (tcp_socket, _remote_address) = 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));
let mut tcp_socket_wrapper = Socket {
send_bytes: |bytes| {
let mut soc = tcp_socket.lock().unwrap();
soc.write(bytes.as_slice()).unwrap();
println!("send bytes");
},
receive_bytes: |bytes| {
let mut soc = tcp_socket.lock().unwrap();
let mut buffer = vec![0 as u8; 3000];
let num_bytes = soc.read(&mut buffer).unwrap();
buffer.resize(num_bytes, 0);
assert_eq!(buffer.len(), num_bytes);
*bytes = buffer;
},
};
let request = self.receive_robot_connect_request(&mut tcp_socket_wrapper);
let udp_port = request.request.udp_port;
self.send_robot_connect_response(request, &mut tcp_socket_wrapper);
let udp_socket = std::net::UdpSocket::bind(
format!("{}:1833", hostname)
.to_socket_addrs()
.unwrap()
.next()
.unwrap(),
)
.unwrap();
udp_socket
.connect(
format!("{}:{}", hostname, udp_port)
.to_socket_addrs()
.unwrap()
.next()
.unwrap(),
)
.unwrap();
let udp_socket_wrapper = Socket {
send_bytes: move |bytes| {
let res = udp_socket.send(bytes.as_slice());
if res.is_err() {
return;
}
let num_bytes = res.unwrap();
assert_eq!(num_bytes, bytes.len());
},
receive_bytes: |_bytes| unimplemented!(),
};
let udp_thread = std::thread::spawn(move || {
let mut counter = 1;
let start = Instant::now();
while (Instant::now() - start).as_secs_f64() < 0.1 {
let mut state = RobotStateIntern::dummy();
state.message_id = counter;
let bytes = serialize(&state).unwrap();
counter += 1;
(udp_socket_wrapper.send_bytes)(&bytes);
std::thread::sleep(Duration::from_millis(5));
}
});
let mut wrapper = Box::new(tcp_socket_wrapper);
for _ in 0..reaction.number_of_reactions() {
self.handle_receive(&mut wrapper, reaction);
std::thread::sleep(Duration::from_millis(5));
}
udp_thread.join().unwrap();
}
fn handle_receive<F, G>(
&self,
tcp_socket: &mut Box<Socket<F, G>>,
reaction: &mut MockServerReaction,
) where
F: Fn(&Vec<u8>),
G: Fn(&mut Vec<u8>),
{
let mut bytes = vec![0 as u8; 100];
(tcp_socket.receive_bytes)(&mut bytes);
let response = reaction.process_received_bytes(&mut bytes);
(tcp_socket.send_bytes)(&response);
}
fn receive_robot_connect_request<F: Fn(&Vec<u8>), G: Fn(&mut Vec<u8>)>(
&self,
tcp_socket: &mut Socket<F, G>,
) -> ConnectRequestWithHeader {
let mut bytes = vec![0 as u8; 100];
(tcp_socket.receive_bytes)(&mut bytes);
let request: ConnectRequestWithHeader = deserialize(bytes.as_slice()).unwrap();
return request;
}
fn send_robot_connect_response<F: Fn(&Vec<u8>), G: Fn(&mut Vec<u8>)>(
&self,
request: ConnectRequestWithHeader,
tcp_socket: &mut Socket<F, G>,
) {
let mut response = ConnectResponse {
header: RobotCommandHeader {
command: RobotCommandEnum::kConnect,
command_id: request.get_command_message_id(),
size: 0,
},
status: match self.server_version == request.request.version {
true => ConnectStatus::kSuccess,
false => ConnectStatus::kIncompatibleLibraryVersion,
},
version: self.server_version,
};
let response_size = serialized_size(&response).unwrap();
response.header.size = response_size as u32;
let serialized_response = serialize(&response).unwrap();
(tcp_socket.send_bytes)(&serialized_response);
}
}
#[test]
fn set_collision_behavior_test() -> FrankaResult<()> {
let mut counter = 0;
let collision_behavior_request_values = [(
[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 mut generate_collision_behavior_request =
move |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]| {
counter += 1;
SetCollisionBehaviorRequestWithHeader {
header: RobotCommandHeader::new(
RobotCommandEnum::kSetCollisionBehavior,
counter,
size_of::<SetCollisionBehaviorRequestWithHeader>() as u32,
),
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 requests = Arc::new(Vec::from_iter(
collision_behavior_request_values
.iter()
.map(|(a, b, c, d, e, f, g, h)| {
generate_collision_behavior_request(*a, *b, *c, *d, *e, *f, *g, *h)
}),
));
let requests_server = requests.clone();
let thread = std::thread::spawn(|| {
let mut robot_server = RobotMockServer::new(kVersion);
let mut mock = MockServerReaction::default();
let num_requests = requests_server.len();
let mut counter = 0;
mock.expect_process_received_bytes()
.returning(move |bytes: &mut Vec<u8>| -> Vec<u8> {
let expected_request = requests_server.get(counter).unwrap();
let serialized_expected_request = serialize(expected_request).unwrap();
assert_eq!(bytes.len(), serialized_expected_request.len());
bytes
.iter()
.zip(serialized_expected_request.iter())
.for_each(|(x, y)| assert_eq!(x, y));
let req: SetCollisionBehaviorRequestWithHeader = deserialize(&bytes).unwrap();
counter += 1;
let mut response = SetCollisionBehaviorResponse {
header: RobotCommandHeader::new(
RobotCommandEnum::kSetCollisionBehavior,
req.header.command_id,
0,
),
status: GetterSetterStatus::kSuccess,
};
response.header.size = serialized_size(&response).unwrap() as u32;
serialize(&response).unwrap()
})
.times(num_requests);
mock.expect_number_of_reactions().return_const(num_requests);
robot_server.server_thread(&mut mock);
});
{
std::thread::sleep(Duration::from_secs_f64(0.01));
let mut robot = Robot::new("127.0.0.1", None, None).expect("robot failure");
assert_eq!(robot.server_version(), kVersion);
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)
.unwrap();
}
}
thread.join().unwrap();
Ok(())
}
#[test]
fn fail_start_motion_test() {
let requests = Arc::new(vec![MoveRequestWithHeader {
header: RobotCommandHeader::new(
RobotCommandEnum::kMove,
1,
size_of::<MoveRequestWithHeader>() as u32,
),
request: MoveRequest::new(
MoveControllerMode::kJointImpedance,
MoveMotionGeneratorMode::kJointPosition,
MoveDeviation {
translation: 10.,
rotation: 3.12,
elbow: 2. * std::f64::consts::PI,
},
MoveDeviation {
translation: 10.,
rotation: 3.12,
elbow: 2. * std::f64::consts::PI,
},
),
}]);
let requests_server = requests.clone();
let thread = std::thread::spawn(|| {
let mut robot_server = RobotMockServer::new(kVersion);
let mut mock = MockServerReaction::default();
let num_requests = requests_server.len();
let mut counter = 0;
mock.expect_process_received_bytes()
.returning(move |bytes: &mut Vec<u8>| -> Vec<u8> {
let expected_request = requests_server.get(counter).unwrap();
let serialized_expected_request = serialize(expected_request).unwrap();
let req: MoveRequestWithHeader = deserialize(&bytes).unwrap();
assert_eq!(bytes.len(), serialized_expected_request.len());
bytes
.iter()
.zip(serialized_expected_request.iter())
.for_each(|(x, y)| assert_eq!(x, y));
counter += 1;
let mut response = MoveResponse {
header: RobotCommandHeader::new(
RobotCommandEnum::kMove,
req.header.command_id,
0,
),
status: MoveStatus::kAborted,
};
response.header.size = serialized_size(&response).unwrap() as u32;
serialize(&response).unwrap()
})
.times(num_requests);
mock.expect_number_of_reactions().return_const(num_requests);
robot_server.server_thread(&mut mock);
});
{
std::thread::sleep(Duration::from_secs_f64(0.01));
let mut robot =
Robot::new("127.0.0.1", RealtimeConfig::kIgnore, None).expect("robot failure");
let mut counter = 0;
let result = robot.control_joint_positions(
|_, _| {
counter += 1;
if counter > 2 {
return JointPositions::new([0.; 7]).motion_finished();
}
JointPositions::new([0.; 7])
},
None,
None,
None,
);
match result {
Err(FrankaException::CommandException { message: _ }) => {
thread.join().unwrap();
}
_ => {
panic!("did not receive a command exception")
}
}
}
}
#[test]
fn incompatible_library() {
let thread = std::thread::spawn(|| {
let mut robot_server = RobotMockServer::new(kVersion + 1);
let mut mock = MockServerReaction::default();
mock.expect_process_received_bytes()
.returning(|_bytes| Vec::<u8>::new());
mock.expect_number_of_reactions().return_const(0 as usize);
robot_server.server_thread(&mut mock);
});
std::thread::sleep(Duration::from_secs_f64(0.01));
let robot_result = Robot::new("127.0.0.1", None, None);
thread.join().unwrap();
match robot_result {
Ok(_) => {
panic!("Expected incompatible library version")
}
Err(error) => match error {
FrankaException::IncompatibleLibraryVersionError { .. } => {}
e => {
panic!("Expected incompatible library version but found {:?}", e)
}
},
};
}
#[test]
fn robot_read_once() -> FrankaResult<()> {
let thread = std::thread::spawn(|| {
let mut robot_server = RobotMockServer::new(kVersion);
let mut mock = MockServerReaction::default();
mock.expect_process_received_bytes()
.returning(|_bytes| Vec::<u8>::new());
mock.expect_number_of_reactions().return_const(0 as usize);
robot_server.server_thread(&mut mock);
});
{
std::thread::sleep(Duration::from_secs_f64(0.01));
let mut robot = Robot::new("127.0.0.1", None, None)?;
let _state = robot.read_once().unwrap();
}
thread.join().unwrap();
Ok(())
}
#[test]
fn robot_read() -> FrankaResult<()> {
let thread = std::thread::spawn(|| {
let mut robot_server = RobotMockServer::new(kVersion);
let mut mock = MockServerReaction::default();
mock.expect_process_received_bytes()
.returning(|_bytes| Vec::<u8>::new());
mock.expect_number_of_reactions().return_const(0 as usize);
robot_server.server_thread(&mut mock);
});
{
std::thread::sleep(Duration::from_secs_f64(0.01));
let mut robot = Robot::new("127.0.0.1", None, None)?;
let mut counter = 0;
let mut first_time = true;
let mut start_counter = 0;
robot
.read(|state: &RobotState| {
if first_time {
first_time = false;
counter = state.time.as_millis();
start_counter = counter;
}
assert_eq!(state.time.as_millis(), counter);
counter += 1;
counter < start_counter + 10
})
.unwrap();
}
thread.join().unwrap();
Ok(())
}
}