franka/joint_velocity_limits/
mod.rs

1//! Position-dependent joint velocity limits read from the robot's URDF.
2//!
3//! Port of libfranka 0.21.2 `include/franka/joint_velocity_limits.h` and
4//! `src/joint_velocity_limits.cpp`. libfranka parses the URDF returned by the `GetRobotModel`
5//! command with tinyxml2; this port uses `roxmltree`, because the
6//! `<position_based_velocity_limits>` element is a vendor extension that `urdf-rs` drops.
7//!
8//! The limits are the same curves that the (deprecated)
9//! [`crate::rate_limiting::compute_upper_limits_joint_velocity`] hardcodes for the FR3, with the
10//! parameters taken from the URDF instead:
11//!
12//! ```text
13//! upper(q) = min(max_velocity, max(0, -velocity_offset + sqrt(max(0, 2 * deceleration_limit * (upper_position_limit - q))))) - tolerance
14//! lower(q) = max(-max_velocity, min(0,  velocity_offset - sqrt(max(0, 2 * deceleration_limit * (q - lower_position_limit))))) + tolerance
15//! ```
16
17use crate::error::{FrankaError, FrankaResult};
18use crate::math_utils::{cmax, cmin};
19use crate::rate_limiting::JOINT_VELOCITY_LIMITS_TOLERANCE;
20
21/// Number of joints. Port of `JointVelocityLimitsConfig::kNumJoints`.
22pub const NUM_JOINTS: usize = 7;
23
24const ROBOT_ELEMENT_NAME: &str = "robot";
25const JOINT_ELEMENT_NAME: &str = "joint";
26const POSITION_BASED_VELOCITY_LIMITS_ELEMENT_NAME: &str = "position_based_velocity_limits";
27const LIMIT_ELEMENT_NAME: &str = "limit";
28const NAME_ATTRIBUTE_NAME: &str = "name";
29const VELOCITY_ATTRIBUTE_NAME: &str = "velocity";
30const UPPER_ATTRIBUTE_NAME: &str = "upper";
31const LOWER_ATTRIBUTE_NAME: &str = "lower";
32const VELOCITY_OFFSET_ATTRIBUTE_NAME: &str = "velocity_offset";
33const DECELERATION_LIMIT_ATTRIBUTE_NAME: &str = "deceleration_limit";
34
35/// Joint name patterns, index 0..6. Port of `kJoint1Name` .. `kJoint7Name`.
36const JOINT_NAMES: [&str; NUM_JOINTS] = [
37    "joint1", "joint2", "joint3", "joint4", "joint5", "joint6", "joint7",
38];
39
40/// Position-based joint velocity limit constants for a single joint, read from the URDF.
41///
42/// Port of `franka::PositionBasedJointVelocityLimitConstants`.
43#[derive(Debug, Clone, Copy, PartialEq, Default)]
44pub struct PositionBasedJointVelocityLimitConstants {
45    /// Maximum velocity (URDF `<limit velocity>`).
46    pub max_velocity: f64,
47    /// Velocity offset (URDF `<position_based_velocity_limits velocity_offset>`).
48    pub velocity_offset: f64,
49    /// Deceleration limit (URDF `<position_based_velocity_limits deceleration_limit>`).
50    pub deceleration_limit: f64,
51    /// Upper position limit (URDF `<limit upper>`).
52    pub upper_position_limit: f64,
53    /// Lower position limit (URDF `<limit lower>`).
54    pub lower_position_limit: f64,
55}
56
57impl PositionBasedJointVelocityLimitConstants {
58    /// Creates the constants for one joint.
59    pub fn new(
60        max_velocity: f64,
61        velocity_offset: f64,
62        deceleration_limit: f64,
63        upper_position_limit: f64,
64        lower_position_limit: f64,
65    ) -> Self {
66        PositionBasedJointVelocityLimitConstants {
67            max_velocity,
68            velocity_offset,
69            deceleration_limit,
70            upper_position_limit,
71            lower_position_limit,
72        }
73    }
74}
75
76/// Position-based joint velocity limit parameters for all seven joints.
77///
78/// Port of `franka::JointVelocityLimitsConfig`.
79#[derive(Debug, Clone, Copy, PartialEq, Default)]
80pub struct JointVelocityLimitsConfig {
81    joint_params: [PositionBasedJointVelocityLimitConstants; NUM_JOINTS],
82}
83
84impl JointVelocityLimitsConfig {
85    /// Parses the joint velocity limit parameters from a URDF string.
86    ///
87    /// Port of `JointVelocityLimitsConfig::parseFromURDF`. Every `<joint>` element whose name
88    /// matches (exactly, or as a substring, so `fr3_joint1` matches too) one of `joint1` ..
89    /// `joint7` must carry a `<limit>` element with `velocity`, `upper` and `lower` attributes
90    /// and a `<position_based_velocity_limits>` element with `velocity_offset` and
91    /// `deceleration_limit` attributes; all seven joints must be present.
92    ///
93    /// # Errors
94    /// [`FrankaError::InvalidArgument`] (libfranka throws `std::runtime_error`) if the string is
95    /// not valid XML, has no `<robot>` root, misses one of the elements, attributes or joints,
96    /// or if an attribute is not a number.
97    pub fn from_urdf(urdf: &str) -> FrankaResult<Self> {
98        // tinyxml2 tolerates whitespace in front of the XML declaration, roxmltree does not.
99        let document = roxmltree::Document::parse(urdf.trim_start()).map_err(|_| {
100            FrankaError::InvalidArgument(
101                "Failed to parse URDF for joint velocity limits".to_string(),
102            )
103        })?;
104        let robot = document.root_element();
105        if robot.tag_name().name() != ROBOT_ELEMENT_NAME {
106            return Err(FrankaError::InvalidArgument(
107                "Failed to parse URDF: no <robot> element exists for joint velocity limits"
108                    .to_string(),
109            ));
110        }
111
112        let mut joint_params = [PositionBasedJointVelocityLimitConstants::default(); NUM_JOINTS];
113        let mut found_joints = [false; NUM_JOINTS];
114
115        for joint in robot
116            .children()
117            .filter(|n| n.is_element() && n.tag_name().name() == JOINT_ELEMENT_NAME)
118        {
119            let Some(name) = joint.attribute(NAME_ATTRIBUTE_NAME) else {
120                continue;
121            };
122            let Some(index) = joint_index(name) else {
123                continue;
124            };
125
126            let position_based_limits = joint
127                .children()
128                .find(|n| {
129                    n.is_element()
130                        && n.tag_name().name() == POSITION_BASED_VELOCITY_LIMITS_ELEMENT_NAME
131                })
132                .ok_or_else(|| {
133                    FrankaError::InvalidArgument(format!(
134                        "Missing <{POSITION_BASED_VELOCITY_LIMITS_ELEMENT_NAME}> element for \
135                         joint: {name}"
136                    ))
137                })?;
138            let limit = joint
139                .children()
140                .find(|n| n.is_element() && n.tag_name().name() == LIMIT_ELEMENT_NAME)
141                .ok_or_else(|| {
142                    FrankaError::InvalidArgument(format!(
143                        "Missing <{LIMIT_ELEMENT_NAME}> element for joint: {name}"
144                    ))
145                })?;
146
147            found_joints[index] = true;
148            joint_params[index] = PositionBasedJointVelocityLimitConstants::new(
149                parse_required_double(&limit, VELOCITY_ATTRIBUTE_NAME, name, LIMIT_ELEMENT_NAME)?,
150                parse_required_double(
151                    &position_based_limits,
152                    VELOCITY_OFFSET_ATTRIBUTE_NAME,
153                    name,
154                    POSITION_BASED_VELOCITY_LIMITS_ELEMENT_NAME,
155                )?,
156                parse_required_double(
157                    &position_based_limits,
158                    DECELERATION_LIMIT_ATTRIBUTE_NAME,
159                    name,
160                    POSITION_BASED_VELOCITY_LIMITS_ELEMENT_NAME,
161                )?,
162                parse_required_double(&limit, UPPER_ATTRIBUTE_NAME, name, LIMIT_ELEMENT_NAME)?,
163                parse_required_double(&limit, LOWER_ATTRIBUTE_NAME, name, LIMIT_ELEMENT_NAME)?,
164            );
165        }
166
167        if !found_joints.iter().all(|found| *found) {
168            let mut missing_joints = String::from("Missing required joints: ");
169            for (i, found) in found_joints.iter().enumerate() {
170                if !found {
171                    missing_joints.push_str(&format!("joint{} ", i + 1));
172                }
173            }
174            return Err(FrankaError::InvalidArgument(missing_joints));
175        }
176
177        Ok(JointVelocityLimitsConfig { joint_params })
178    }
179
180    /// Joint velocity limit parameters of all joints.
181    ///
182    /// Port of `JointVelocityLimitsConfig::getJointParams`.
183    pub fn joint_params(&self) -> &[PositionBasedJointVelocityLimitConstants; NUM_JOINTS] {
184        &self.joint_params
185    }
186
187    /// Computes the upper joint velocity limits at the given joint positions.
188    ///
189    /// Port of `JointVelocityLimitsConfig::getUpperJointVelocityLimits`.
190    pub fn upper_limits(&self, q: &[f64; NUM_JOINTS]) -> [f64; NUM_JOINTS] {
191        let mut result = [0.0; NUM_JOINTS];
192        for i in 0..NUM_JOINTS {
193            let params = &self.joint_params[i];
194            result[i] = cmin(
195                params.max_velocity,
196                cmax(
197                    0.0,
198                    -params.velocity_offset
199                        + cmax(
200                            0.0,
201                            2.0 * params.deceleration_limit * (params.upper_position_limit - q[i]),
202                        )
203                        .sqrt(),
204                ),
205            ) - JOINT_VELOCITY_LIMITS_TOLERANCE[i];
206        }
207        result
208    }
209
210    /// Computes the lower joint velocity limits at the given joint positions.
211    ///
212    /// Port of `JointVelocityLimitsConfig::getLowerJointVelocityLimits`.
213    pub fn lower_limits(&self, q: &[f64; NUM_JOINTS]) -> [f64; NUM_JOINTS] {
214        let mut result = [0.0; NUM_JOINTS];
215        for i in 0..NUM_JOINTS {
216            let params = &self.joint_params[i];
217            result[i] = cmax(
218                -params.max_velocity,
219                cmin(
220                    0.0,
221                    params.velocity_offset
222                        - cmax(
223                            0.0,
224                            2.0 * params.deceleration_limit * (-params.lower_position_limit + q[i]),
225                        )
226                        .sqrt(),
227                ),
228            ) + JOINT_VELOCITY_LIMITS_TOLERANCE[i];
229        }
230        result
231    }
232}
233
234impl std::ops::Index<usize> for JointVelocityLimitsConfig {
235    type Output = PositionBasedJointVelocityLimitConstants;
236
237    /// Port of `JointVelocityLimitsConfig::operator[]`.
238    fn index(&self, joint_index: usize) -> &Self::Output {
239        &self.joint_params[joint_index]
240    }
241}
242
243/// Joint index for a joint name, or `None`.
244///
245/// Port of `JointVelocityLimitsConfig::getJointIndex`: exact match first, then substring match,
246/// so that prefixed names such as `fr3_joint1` are recognised as well.
247fn joint_index(joint_name: &str) -> Option<usize> {
248    if let Some(index) = JOINT_NAMES.iter().position(|name| *name == joint_name) {
249        return Some(index);
250    }
251    JOINT_NAMES
252        .iter()
253        .position(|pattern| joint_name.contains(pattern))
254}
255
256/// Port of the `parse_required_double` lambda in `parseFromURDF`.
257fn parse_required_double(
258    element: &roxmltree::Node<'_, '_>,
259    attribute: &str,
260    joint_name: &str,
261    element_name: &str,
262) -> FrankaResult<f64> {
263    let value = element.attribute(attribute).ok_or_else(|| {
264        FrankaError::InvalidArgument(format!(
265            "Missing '{attribute}' attribute in <{element_name}> for joint: {joint_name}"
266        ))
267    })?;
268    value.trim().parse::<f64>().map_err(|_| {
269        FrankaError::InvalidArgument(format!(
270            "Invalid '{attribute}' attribute in <{element_name}> for joint: {joint_name}"
271        ))
272    })
273}
274
275#[cfg(test)]
276mod tests;
franka/joint_velocity_limits/mod.rs

franka/joint_velocity_limits/
mod.rs
