pyrcf.components.controllers.segway_pid_balance_controller

Adapted from wheel_controller from Stéphane Caron’s upkie repo. (https://github.com/upkie/upkie/blob/main/pid_balancer/wheel_controller.py).

Most of the comments from the original code still included.

# License from upkie repo code included below:

Classes

SegwayPIDBalanceController

A simple PD balance controller for a 2-wheel segway type robot.

Module Contents

class pyrcf.components.controllers.segway_pid_balance_controller.SegwayPIDBalanceController(pinocchio_interface: pyrcf.utils.kinematics_dynamics.pinocchio_interface.PinocchioInterface, gains: Gains = Gains(), air_return_period: float = 1.0, fall_pitch: float = 1.0, max_ground_velocity: float = 2.0, max_integral_error_velocity: float = 10.0, max_target_accel: float = 0.3, max_target_distance: float = 1.5, max_target_velocity: float = 0.6, max_yaw_accel: float = 10.0, max_yaw_velocity: float = 1.0, turning_deadband: float = 0.015, turning_decision_time: float = 0.2, wheel_radius: float = 0.06, wheel_distance: float = 0.3048, wheel_ee_ids: List[int] = [0, 1], wheel_joint_ids: List[int] = [2, 5], wheel_joint_directions: List[int] = [1, -1], joint_torque_limits: List[float] = [-1.0, 1.0])

Bases: pyrcf.components.controllers.controller_base.ControllerBase

A simple PD balance controller for a 2-wheel segway type robot.

class Gains

Gains for this controller.

Parameters:

pitch_damping – Pitch error (normalized) damping gain. Corresponds to the proportional term of the velocity PI controller, equivalent to the derivative term of the acceleration PD controller.
pitch_stiffness – Pitch error (normalized) stiffness gain. Corresponds to the integral term of the velocity PI controller, equivalent to the proportional term of the acceleration PD controller.
footprint_position_damping – Position error (normalized) damping gain. Corresponds to the proportional term of the velocity PI controller, equivalent to the derivative term of the acceleration PD controller.
footprint_position_stiffness – Position error (normalized) stiffness gain. Corresponds to the integral term of the velocity PI controller, equivalent to the proportional term of the acceleration PD controller.
joint_kp – Joint position tracking gain (for non-wheel joints).
joint_kd – Wheel joint velocity gain (only for wheel joints)
turning_gain_scale – joint velocity additional gains when there is turning probability (yaw command).

pitch_damping: float = 10.0

pitch_stiffness: float = 20.0

footprint_position_damping: float = 5.0

footprint_position_stiffness: float = 2.0

joint_kp: float = 200.0

joint_kd: float = 2.0

turning_gain_scale: float = 2.0

air_return_period = 1.0

fall_pitch = 1.0

gains

integral_error_velocity = 0.0

max_ground_velocity = 2.0

max_integral_error_velocity = 10.0

max_target_accel = 0.3

max_target_distance = 1.5

max_target_velocity = 0.6

max_yaw_accel = 10.0

max_yaw_velocity = 1.0

pitch = 0.0

target_ground_position = 0.0

target_ground_velocity = 0.0

target_yaw_position = 0.0

target_yaw_velocity = 0.0

turning_deadband = 0.015

turning_decision_time = 0.2

turning_probability = 0.0

wheel_radius = 0.06

wheel_ee_ids = [0, 1]

wheel_distance = 0.3048

wheel_joint_ids = [2, 5]

torque_lims

wheel_joint_directions

_pin

kp_ground_vel

ki_ground_vel

_reference_joint_pos = None

_cmd: pyrcf.core.types.RobotCmd = None

compute_wheel_velocities(robot_state: pyrcf.core.types.RobotState, local_plan: pyrcf.core.types.LocalMotionPlan, dt: float = None) → numpy.ndarray

update(robot_state: pyrcf.core.types.RobotState, local_plan: pyrcf.core.types.LocalMotionPlan, t: float, dt: float) → pyrcf.core.types.RobotCmd

The main control update method to be called in the loop.

This method will be called after the global plan and local plan are generated in the loop.

This method should be implemented by any controller implementation.

Parameters:

robot_state (RobotState) – The current state information from the robot.
local_plan (LocalMotionPlan) – the latest local plan generated by the local planner used in the loop.
t (float, optional) – the current time signature of the control loop. Defaults to None (controllers may or may not need this).
dt (float, optional) – the time since the last control loop. Defaults to None (controllers may or may not need this).

Raises:

NotImplementedError – Raised if this method is not implemented by the child class.

Returns:

The output control command to be sent to the robot.

Return type:

RobotCmd