pyrcf.components.controllers

Module containing interface and definitions of controllers that can be used in the control loop.

Submodules

• pyrcf.components.controllers.controller_base
• pyrcf.components.controllers.gravity_compensated_joint_pd_controller
• pyrcf.components.controllers.joint_pd_controller
• pyrcf.components.controllers.segway_pid_balance_controller

Classes

ControllerBase	Base class defining the interface for all controllers that can be used in the control loop.
DummyController	Dummy controller for testing pipeline.
JointPDController	Tracks joint position and velocity reference provided in local_plan message directly.
GravityCompensatedPDController	An extension of the JointPDController that adds gravity compensation
SegwayPIDBalanceController	A simple PD balance controller for a 2-wheel segway type robot.

Package Contents

class pyrcf.components.controllers.ControllerBase

Bases: pyrcf.components.pyrcf_component.PyRCFComponent

Base class defining the interface for all controllers that can be used in the control loop.

abstract update(robot_state: pyrcf.core.types.RobotState, local_plan: pyrcf.core.types.LocalMotionPlan, t: float = None, dt: float = None) → 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

class pyrcf.components.controllers.DummyController(squawk: bool = True)

Bases: ControllerBase

Dummy controller for testing pipeline.

_squawk = True

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

This is a dummy method for sanity checking the control loop.

class pyrcf.components.controllers.JointPDController(kp: numpy.ndarray | float, kd: numpy.ndarray | float, hold_position_at_start: bool = True, gain_ramp_up_time: float = 0.0, ramp_start_kp: numpy.ndarray | float = 0.0, ramp_start_kd: numpy.ndarray | float = 0.0)

Bases: pyrcf.components.controllers.controller_base.ControllerBase

Tracks joint position and velocity reference provided in local_plan message directly.

_target_kp

_target_kd

_hold_position = True

_ramp_duration = 0.0

_ramp_start_kp

_ramp_start_kd

_on_ramp_target

_ramp_factor = 1.0

_duration_in_ctrl_mode = 0.0

_ramping_complete = False

_robot_cmd: pyrcf.core.types.RobotCmd = None

update(robot_state: pyrcf.core.types.RobotState, local_plan: pyrcf.core.types.LocalMotionPlan, t: float = None, dt: float = None) → 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

class pyrcf.components.controllers.GravityCompensatedPDController(kp: numpy.ndarray | float, kd: numpy.ndarray | float, pinocchio_interface: pyrcf.utils.kinematics_dynamics.pinocchio_interface.PinocchioInterface, hold_position_at_start: bool = True, gain_ramp_up_time: float = 0.0, ramp_start_kp: numpy.ndarray | float = 0.0, ramp_start_kd: numpy.ndarray | float = 0.0, ramp_gravity_torques: bool = True)

Bases: pyrcf.components.controllers.joint_pd_controller.JointPDController

An extension of the JointPDController that adds gravity compensation torques to the commands.

_ramp_gravity = True

_pin_iface

_compensate = True

_j_id_offset = 6

toggle_compensation(enable: bool = None)

Enable or disable gravity compensation by applying additional torque commands to compensate robot’s static weight.

Parameters:

enable (bool, optional) – True to enable, False to disable. Defaults to None, (when None provided, will automatically toggle between enable/disable).

update(robot_state: pyrcf.core.types.RobotState, local_plan: pyrcf.core.types.LocalMotionPlan, t: float = None, dt: float = None) → 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

class pyrcf.components.controllers.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