Skip to content

Commit

Permalink
implement rate limiter directly in Fr3 and Panda struct
Browse files Browse the repository at this point in the history
  • Loading branch information
Marco Boneberger committed Oct 17, 2023
1 parent 9863d0c commit 444b6b7
Show file tree
Hide file tree
Showing 8 changed files with 377 additions and 357 deletions.
73 changes: 63 additions & 10 deletions src/robot.rs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;

Expand Down Expand Up @@ -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.
Expand Down Expand Up @@ -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.
Expand Down Expand Up @@ -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.
Expand Down Expand Up @@ -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.
Expand Down Expand Up @@ -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`)
Expand Down Expand Up @@ -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.
Expand Down Expand Up @@ -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.
Expand Down Expand Up @@ -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.
Expand Down Expand Up @@ -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.
Expand Down
11 changes: 8 additions & 3 deletions src/robot/control_loop.rs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -199,7 +199,7 @@ impl<
}
if self.limit_rate {
control_output.tau_J = limit_rate_torques(
&Data::RateLimiterParameters::MAX_TORQUE_RATE,
&<Data as RateLimiter>::MAX_TORQUE_RATE,
&control_output.tau_J,
&robot_state.get_tau_J_d(),
);
Expand All @@ -215,7 +215,12 @@ impl<
command: &mut MotionGeneratorCommand,
) -> bool {
let motion_output = (self.motion_callback)(robot_state, time_step);
motion_output.convert_motion::<Data::RateLimiterParameters>(robot_state, command, self.cutoff_frequency, self.limit_rate);
motion_output.convert_motion::<Data>(
robot_state,
command,
self.cutoff_frequency,
self.limit_rate,
);
!motion_output.is_finished()
}
}
12 changes: 6 additions & 6 deletions src/robot/control_types.rs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -37,7 +37,7 @@ pub enum RealtimeConfig {

pub(crate) trait ConvertMotion<State: AbstractRobotState> {
/// converts the motion type to a MotionGeneratorCommand and applies rate limiting and filtering
fn convert_motion<Params: RateLimiterParameters>(
fn convert_motion<Params: RateLimiter>(
&self,
robot_state: &State,
command: &mut MotionGeneratorCommand,
Expand Down Expand Up @@ -140,7 +140,7 @@ impl Finishable for JointPositions {
}
}
impl ConvertMotion<RobotState> for JointPositions {
fn convert_motion<Params: RateLimiterParameters>(
fn convert_motion<Params: RateLimiter>(
&self,
robot_state: &RobotState,
command: &mut MotionGeneratorCommand,
Expand Down Expand Up @@ -215,7 +215,7 @@ impl Finishable for JointVelocities {
}
}
impl ConvertMotion<RobotState> for JointVelocities {
fn convert_motion<Params: RateLimiterParameters>(
fn convert_motion<Params: RateLimiter>(
&self,
robot_state: &RobotState,
command: &mut MotionGeneratorCommand,
Expand Down Expand Up @@ -331,7 +331,7 @@ impl Finishable for CartesianPose {
}

impl ConvertMotion<RobotState> for CartesianPose {
fn convert_motion<Params: RateLimiterParameters>(
fn convert_motion<Params: RateLimiter>(
&self,
robot_state: &RobotState,
command: &mut MotionGeneratorCommand,
Expand Down Expand Up @@ -453,7 +453,7 @@ impl Finishable for CartesianVelocities {
}
}
impl ConvertMotion<RobotState> for CartesianVelocities {
fn convert_motion<Params: RateLimiterParameters>(
fn convert_motion<Params: RateLimiter>(
&self,
robot_state: &RobotState,
command: &mut MotionGeneratorCommand,
Expand Down
Loading

0 comments on commit 444b6b7

Please sign in to comment.