platform_utils

Mobile platform helper utilities.

This module provides helper functions for reduced kinematics and fully local. vector-field control of a nonholonomic mobile platform tracking a manipulator TCP treated as a point (pointer). The reduced kinematic model uses only the first five joints of a Franka Emika Panda arm to compute the translational TCP position and the corresponding geometric Jacobian. These quantities are then used to derive distance-to-workspace metrics and to generate smooth, rate-limited linear and angular velocity commands for the mobile base directly in its local frame, without relying on global localization or onboard sensing.

Key functionalities include: - Reduced 5-DoF Panda forward kinematics for translational TCP position and Jacobian. - Signed distance computations to spherical and sphere-or-cylinder workspace boundaries. - Angle wrapping and smooth activation functions for rotational alignment. - Rate limiting and exponential smoothing for stable velocity commands. - Vector-field-based tracking for forward, hold, and retreat behaviors.

robotblockset.platform_utils.distance_to_sphere_or_cylinder(p: ndarray | List[float] | List[int] | Tuple[float, ...] | Tuple[int, ...], h1: float, s1: float) → float[source]

Compute the signed distance to a sphere-or-cylinder workspace boundary.

The workspace is modeled as a sphere centered at [0, 0, h1] with radius s1 for points above z = h1, and as an infinite cylinder aligned with the z axis with the same radius for points at or below z = h1.

Parameters:

  • p (ArrayLike) – Cartesian point [x, y, z] expressed in the workspace frame.

  • h1 (float) – Height of the spherical cap center along the z axis.

  • s1 (float) – Radius of both the sphere and the cylinder.

Returns:

Signed distance to the boundary. Positive values indicate that the point lies inside the modeled workspace, negative values indicate that it lies outside, and zero corresponds to the boundary.

Return type:

float

robotblockset.platform_utils.distance_to_sphere(p: ndarray | List[float] | List[int] | Tuple[float, ...] | Tuple[int, ...], height: float, radius: float) → float[source]

Compute the signed distance to a spherical workspace boundary.

Parameters:

  • p (ArrayLike) – Cartesian point [x, y, z] expressed in the workspace frame.

  • height (float) – z coordinate of the sphere center.

  • radius (float) – Sphere radius.

Returns:

Signed distance to the sphere surface. Positive values indicate that the point lies inside the sphere, negative values indicate that it lies outside, and zero corresponds to the surface.

Return type:

float

robotblockset.platform_utils.wrap_to_pi(angle: float) → float[source]

Wrap an angle to the interval [-pi, pi].

Parameters:

angle (float) – Input angle in radians.

Returns:

Angle wrapped to the range [-pi, pi].

Return type:

float

robotblockset.platform_utils.smoothstep(x: float, edge0: float, edge1: float) → float[source]

Compute a smooth activation value between two thresholds.

Parameters:

  • x (float) – Input value.

  • edge0 (float) – Lower transition bound.

  • edge1 (float) – Upper transition bound.

Returns:

Smoothed value in the range [0, 1] with continuous first derivatives inside the transition region.

Return type:

float

robotblockset.platform_utils.limit_rate(value_prev: float, value_target: float, max_delta: float) → float[source]

Limit the step-to-step change of a scalar signal.

Parameters:

  • value_prev (float) – Previously applied signal value.

  • value_target (float) – Desired signal value for the current control step.

  • max_delta (float) – Maximum allowed absolute change between consecutive values.

Returns:

Rate-limited signal value.

Return type:

float

robotblockset.platform_utils.check_panda_reach(robot: robot, **kwargs: Any) → bool[source]

Check whether the reduced Panda TCP is close to the workspace boundary.

A reduced 5-DoF Panda kinematic model is used to evaluate the TCP position and its signed distance to a spherical workspace limit.

Parameters:

  • robot (robot) – robot Robot instance with a valid joint configuration in robot.q.

  • **kwargs (dict) –

    Optional keyword arguments. Supported key:

    d_minfloat, optional

    Minimum allowed signed distance to the workspace boundary.

Returns:

True if the TCP is closer than d_min to the boundary, otherwise False.

Return type:

bool

robotblockset.platform_utils.yaw_from_tcp_direction(R: ndarray, alpha: float, sign: float = 1.0) → float[source]

Compute a world-frame yaw angle from a TCP-frame planar direction.

Parameters:

  • R (RotationMatrixType) – TCP rotation matrix expressed in the world frame.

  • alpha (float) – Direction angle in the TCP xy plane in radians, where 0 corresponds to +x and pi / 2 to +y.

  • sign (float, optional) – Direction multiplier. Use 1.0 for the forward direction and -1.0 for the opposite direction.

Returns:

World-frame yaw angle in radians corresponding to the transformed planar direction vector.

Return type:

float

robotblockset.platform_utils.reconfigure_base_from_tcp(robot: robot, platform: platform, base_goal_mode: str = 'reconfigure', yaw_axis: float = 0.0, task_DOF: ndarray | List[float] | List[int] | Tuple[float, ...] | Tuple[int, ...] = array([0, 0, 1, 1, 1, 0]), null_space_task: str = 'JointLimit', Kns: float = 100, pos_err: float = -1, approach_dist: float = 0.05, return_debug: bool = False, motion_enable: bool = False, **kwargs: Any) → Tuple[ndarray, float] | Tuple[ndarray, float, Dict[str, Any]][source]

Compute a mobile-base target position and yaw from a TCP reconfiguration.

The function evaluates an inverse-kinematics-based manipulator reconfiguration and derives a planar platform target that preserves the current TCP pose. Two goal-generation modes are supported: "reconfigure", which uses the relative transform between the current and reconfigured robot poses, and "tcp_direction", which places the base behind the TCP along a selected tool-frame direction.

Parameters:

  • robot (robot) – robot Manipulator instance attached to the platform.

  • platform (platform) – Mobile platform to be repositioned.

  • base_goal_mode (str, optional) – "reconfigure" and "tcp_direction".

  • yaw_axis (float, optional) – Direction angle in the TCP xy plane, in radians, used when base_goal_mode is "tcp_direction".

  • task_DOF (ArrayLike, optional) – solver.

  • null_space_task (str, optional) – solver.

  • Kns (float, optional) – Null-space gain passed to the inverse kinematics solver.

  • pos_err (float, optional) – Position tolerance passed to the inverse kinematics solver.

  • approach_dist (float, optional) – Final approach distance passed to platform.CMoveToLocation(...) when motion_enable is enabled.

  • return_debug (bool, optional) – If True, return intermediate kinematic quantities together with the platform target.

  • motion_enable (bool, optional) – If True, execute the coordinated robot-platform motion immediately.

  • **kwargs (dict) – Additional keyword arguments reserved for future compatibility.

Returns:

Platform target position pt in the platform plane and target yaw. When return_debug is True, the returned tuple also contains a dictionary with intermediate values used during the computation.

Return type:

tuple[Vector2DType, float] or tuple[Vector2DType, float, dict[str, Any]]

robotblockset.platform_utils.track_panda_vf(platform: platform, K_v: float = 2.0, K_omega: float = 1.0, K_back: float = 2, a_max: float = 1, w_max: float = 1, angle_thresh_deg_low: float = 30.0, angle_thresh_deg_high: float = 45.0, theta_retreat_target_deg: float = 30.0, alpha: float = 0.25, max_omega: float = 1.0, f_zone: float = 0.15, b_zone: float = 0.15, l0: float = 0.333, l1: float = np.float64(0.3280243893371345), l2: float = np.float64(0.39395431207184417)) → int[source]

Run a local vector-field controller for a nonholonomic mobile platform.

This controller drives the platform from the relative position of the robot TCP only. The TCP is modeled as a point obtained from the reduced 5-DoF Panda kinematics. No global localization or environment sensing is required.

Parameters:

  • platform (platform) – Mobile platform instance providing access to platform state, robot state, sample time, and velocity commands.

  • K_v (float, optional) – region.

  • K_omega (float, optional) – Gain for angular velocity used to align the platform with the TCP direction.

  • K_back (float, optional) – Gain for backward velocity during retreat behavior.

  • a_max (float, optional) – Maximum linear acceleration.

  • w_max (float, optional) – Maximum angular acceleration.

  • angle_thresh_deg_low (float, optional) – Lower angular threshold in degrees below which rotation is suppressed.

  • angle_thresh_deg_high (float, optional) – activated.

  • theta_retreat_target_deg (float, optional) – Desired orientation offset in degrees during retreat behavior.

  • alpha (float, optional) – Exponential smoothing factor for the commanded velocities.

  • max_omega (float, optional) – Maximum allowed absolute angular velocity.

  • f_zone (float, optional) – Forward activation threshold near the outer workspace boundary.

  • b_zone (float, optional) – Retreat activation threshold near the inner safety zone.

  • l0 (float, optional) – Base-to-first-joint distance used in the reduced workspace model.

  • l1 (float, optional) – Effective upper-arm length in the reduced workspace model.

  • l2 (float, optional) – Effective forearm length in the reduced workspace model.

Returns:

Status code indicating the termination condition. 0 denotes an abort request from the platform, while 2 indicates that no robot is attached to the platform.

Return type:

int

Functions

check_panda_reach(robot, **kwargs)

Check whether the reduced Panda TCP is close to the workspace boundary.

distance_to_sphere(p, height, radius)

Compute the signed distance to a spherical workspace boundary.

distance_to_sphere_or_cylinder(p, h1, s1)

Compute the signed distance to a sphere-or-cylinder workspace boundary.

limit_rate(value_prev, value_target, max_delta)

Limit the step-to-step change of a scalar signal.

reconfigure_base_from_tcp(robot, platform[, ...])

Compute a mobile-base target position and yaw from a TCP reconfiguration.

smoothstep(x, edge0, edge1)

Compute a smooth activation value between two thresholds.

track_panda_vf(platform[, K_v, K_omega, ...])

Run a local vector-field controller for a nonholonomic mobile platform.

wrap_to_pi(angle)

Wrap an angle to the interval [-pi, pi].

yaw_from_tcp_direction(R, alpha[, sign])

Compute a world-frame yaw angle from a TCP-frame planar direction.