feathersdk.robot.actuator_systems.navigation_ik Module

Planar inverse kinematics for the three-wheel swerve.

Body twist (vx, vy, omega) and module positions (x_i, y_i) use the robot / IMU odometry frame: +X forward, +Y left, yaw positive counter-clockwise about +Z (up).

Steering-motor commands use a different yaw sign: 0 is wheel-forward, +90 deg is right (-Y), and -90 deg is left (+Y). That is clockwise-positive about +Z, so the motor target is -atan2(wheel_vy, wheel_vx).

Drive-motor commands are rad/s: motor_velocity = linear_speed / wheel_radius. Positive drive velocity rolls the wheel along the commanded steering heading.

feathersdk.robot.actuator_systems.navigation_ik.ZERO_VEL_EPS = 1e-09

Treat body / wheel speeds below this as stopped.

feathersdk.robot.actuator_systems.navigation_ik.STEER_LIMIT = 1.5707963267948966

[-90, +90] deg. 0 = wheel pointing forward.

Type:

Kinematic steering operating range

feathersdk.robot.actuator_systems.navigation_ik.STEER_HYSTERESIS = 0.3490658503988659

Keep the last equivalent (angle, ±speed) unless the other is this much closer, radians.

feathersdk.robot.actuator_systems.navigation_ik.wrap_angle(angle)

Wrap angle to (-pi, pi].

Parameters:

angle (float)

Return type:

float

feathersdk.robot.actuator_systems.navigation_ik.wheel_body_velocity(vx, vy, omega, x_i, y_i)

Ground-contact velocity of a module at (x_i, y_i) for a rigid-body planar twist.

(x_i, y_i) and (vx, vy) are in the robot frame (+X forward, +Y left). omega is positive counter-clockwise:

wheel_vx = vx - omega * y_i wheel_vy = vy + omega * x_i

Parameters:
Return type:

Tuple[float, float]

feathersdk.robot.actuator_systems.navigation_ik.steer_target_from_velocity(wheel_vx, wheel_vy, current_angle, angle_range, previous_angle=None, hysteresis=0.3490658503988659)

Map a wheel velocity vector to a steer angle in [-pi/2, pi/2] and a signed linear speed.

(wheel_vx, wheel_vy) is in the robot frame (+X forward, +Y left). The returned steer angle is in the motor frame (0 = forward, +90 deg = right).

A heading outside ±90 deg is not clamped. The two equivalent commands are (angle, speed) and (angle ± pi, -speed) (reverse the drive motor). The in-range representation that requires the least motion from current_angle is chosen, with hysteresis: if previous_angle is set, the last command’s equivalent is kept unless the other option is closer by more than hysteresis. That stops ±90 chatter when the two options are almost equally far.

Returns (steer_angle_rad, linear_speed_m_s). Convert to drive-motor rad/s with / wheel_radius.

Parameters:
Return type:

Tuple[float, float]

feathersdk.robot.actuator_systems.navigation_ik.normalize_motor_velocities(motor_velocities, max_velocities)

Scale all drive-motor velocities uniformly so none exceeds its max_velocity.

Independent per-wheel clamping would change the commanded body motion; a single scale factor preserves direction of (vx, vy, omega).

Parameters:
Return type:

List[float]